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Abstract 

Self-localization  of  an  underwater  vehicle  is  particularly  challenging  due  to  the  ab¬ 
sence  of  Global  Positioning  System  fIGPSI)  reception  or  features  at  known  positions 
that  could  otherwise  have  been  used  for  position  computation.  Thus  Autonomous 
Underwater  Vehicle  fl AT  J VI)  applications  typically  require  the  pre-deployment  of  a  set 
of  beacons. 

This  thesis  examines  the  scenario  in  which  the  members  of  a  group  of  lAUVT 
exchange  navigation  information  with  one  another  so  as  to  improve  their  individual 
position  estimates. 

We  describe  how  the  underwater  environment  poses  unique  challenges  to  vehicle 
navigation  not  encountered  in  other  environments  in  which  robots  operate  and  how 
cooperation  can  improve  the  performance  of  self-localization.  As  intra-vehicle  com¬ 
munication  is  crucial  to  cooperation,  we  also  address  the  constraints  of  the  communi¬ 
cation  channel  and  the  effect  that  these  constraints  have  on  the  design  of  cooperation 
strategies. 

The  classical  approaches  to  underwater  self-localization  of  a  single  vehicle,  as 
well  as  more  recently  developed  techniques  are  presented.  We  then  examine  how 
methods  used  for  cooperating  land-vehicles  can  be  transferred  to  the  underwater 
domain.  An  algorithm  for  distributed  self-localization,  which  is  designed  to  take  the 
specific  characteristics  of  the  environment  into  account,  is  proposed. 

We  also  address  how  correlated  position  estimates  of  cooperating  vehicles  can  lead 
to  overconfidence  in  individual  position  estimates. 

Finally,  key  to  any  successful  cooperative  navigation  strategy  is  the  incorpora¬ 
tion  of  the  relative  positioning  between  vehicles.  The  performance  of  localization 
algorithms  with  different  geometries  is  analyzed  and  a  distributed  algorithm  for  the 
dynamic  positioning  of  vehicles,  which  serve  as  dedicated  navigation  beacons  for  a 
fleet  olTAUVk  is  proposed. 
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Chapter  1 
Introduction 


A  new  wave  of  robots  has  led  to  exciting  scientific  findings  over  the  past  decade.  Long 
after  static  manufacturing  robots  matured  to  become  useful  tools  and  common  sights 
on  most  factory  work  floors,  highly  mobile  exploration  robots  reach  places  which 
were  previously  hard  or  impossible  to  access  by  humans.  They  provide  a  platform 
for  their  onboard  sensors  which  collect  data  in  an  environment  which  has  never  been 
previously  visited. 

Perhaps  the  best  known  examples  of  exploration  robots  are  the  two  Mars  Explo¬ 
ration  Rover  IMERY)  vehicles  that  landed  on  Mars  in  2003  and  the  earlier  Pathfinder 
mission  vehicle  Sojourner.  Before  Sojourner  landed  on  Mars  in  1997,  a  large  number 
of  static  space  probes  had  been  sent  to  several  planets  in  the  solar  system  since  the 
seventies,  but  Pathfinder's  mobility  greatly  increased  the  number  of  possible  appli¬ 
cations.  From  Sojourner  to  \MER\  the  mobility  of  the  planetary  exploration  rovers 
was  increased  by  three  orders  of  magnitude  from  tens  of  meters  to  tens  of  kilometers 
within  six  years.  \MER\  also  demonstrated  the  improvements  made  in  reliability  with 
the  robots  continuing  to  provide  scientific  data  since  2003,  after  more  than  five  years 
of  operation. 

Another  environment  in  which  autonomous  exploration  robots  have  operated  very 
successfully  that  has  gained  much  less  attention  is  in  the  bodies  of  water  covering 
70  %  of  Earth’s  surface.  The  dive  of  the  Trieste ,  a  manned  submersible,  reaching 
the  Challenger  DeptlE  in  1960,  demonstrated  the  potential  to  create  vehicles  capable 
of  reaching  every  spot  in  the  ocean.  The  amount  of  ocean  floor  explored  by  this 
vehicle  however  was  only  a  few  square  meters  and  no  vehicle,  manned  or  unmanned, 
has  returned  to  these  depths  since.  Even  today’s  much  more  sophisticated  craft  are 
not  able  to  explore  farther  than  a  few  kilometers  beyond  the  vehicle’s  position  due 
to  the  strong  attenuation  of  electromagnetic  and  acoustic  waves  in  the  water  body 
which  limits  the  footprint  of  the  vehicles’  sensors.  For  example  a  single  space  probe, 
Mars  Express ,  was  able,  in  just  a  few  years,  to  obtain  a  much  more  detailed  image  of 
the  Martian  surface,  than  a  century  of  ocean  exploration  was  able  to  create  for  the 
Earth’s  seafloor.  While  sensors  for  the  ocean  environment  are  continuously  improving 
and  satellites  can  measure  surface  features  such  as  temperature  and  wave  height,  it 

The  deepest  point  in  the  ocean  at  10916  m  m 


23 


24 


Chapter  1.  Introduction 


Figure  1-1:  MER  and  Seabed ,  two  exploration  robots  in  a  hostile  environment.  Left: 
Artist  impression  of  MER  on  Mars.  Courtesy  NASA/JPL-Caltech.  Right:  Seabed 
deployed  during  the  Gakkel  Ridge  Expedition  in  July  2008.  Photo  courtesy  of  Hanu 
Singh. 


remains  difficult  for  the  foreseeable  future  to  see  through  the  water  column. 

The  Earth’s  oceans  affect  all  of  Earth’s  inhabitants  as  they  strongly  influence 
the  climate  and  provide  much  needed  resources  (e.g.  seafood  as  well  as  oil  and 
minerals).  The  influence  on  the  climate  and  the  effect  of  the  ocean’s  biomass  are  not 
well  understood,  mostly  due  to  the  lack  of  available  data.  Classical  ocean  exploration 
mostly  relies  on  immobile  buoys  and  manned  surface  and  underwater  vehicles.  As 
a  result  of  the  high  cost  and  the  inherent  danger  to  people’s  lives,  the  number  of 
people  and  vessels  deployed  for  ocean  research  world-wide  is  relatively  small.  The 
small  footprint  of  ocean  sensors,  combined  with  the  low  number  of  vehicles  carrying 
them,  limits  the  speed  at  which  the  water  body  can  be  explored. 

With  the  footprint  of  each  vehicle  limited  to  a  small  area  around  its  position, 
the  volume  of  ocean  surveyed  is  proportional  to  the  number  of  vehicles  deployed. 
The  only  option  to  strongly  increase  the  spatial  and  temporal  sample  density  is  to 
deploy  a  much  larger  number  of  vehicles.  The  high  cost  of  manned  vehicles  prohibits 
a  large  increase  in  their  use,  but  autonomous  underwater  and  surface  robots  provide 
an  attractive  alternative.  They  only  require  a  small  amount  of  personnel  during 
deployment  and  recovery  and  their  fuel  and  maintenance  costs  are  marginal  when 
compared  to  those  of  conventional  research  vessels. 

Having  a  good  estimate  of  one’s  location  is  critical  for  manned  and  unmanned 
vehicles,  but  it  is  particularly  important  for  autonomous  vehicles  as  unsupervised 
decisions  are  made  based  on  the  location  estimate.  Furthermore  the  quality  of  the 
data  collected  is  directly  dependent  on  how  well  measurements  can  be  referenced  to 
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a  geographic  location.  The  underwater  environment  makes  determining  a  vehicle’s 
position  particularly  difficult,  as  we  will  show  in  this  thesis,  requiring  new  strategies 
for  navigation.  One  such  strategy,  which  we  propose,  is  the  concept  of  Coopera¬ 
tive  Navigation  flCNH  in  which  a  group  of  autonomous  vehicles  exchanges  navigation 
information  in  order  to  improve  the  group’s  overall  position  estimate.  This  thesis 
investigates  the  challenges  involved  in  implementing  this  concept  and  proposes  solu¬ 
tions. 


1.1  Autonomous  Marine  Vehicles 

In  the  last  20  years,  research  in  autonomous  marine  platforms  has  led  to  a  large  and 
ever  growing  number  of  different  submarine  vehicles.  Autonomous  Surface  Crafts 
(lASCffi  have  only  recently  received  more  attention  and  underwater  platforms  continue 
to  dominate  the  research.  This  thesis  will  show  examples  of  how  a  joint  deployment 
of  lASCfe  and  Autonomous  Underwater  Vehicles  (lAIJVh)  can  be  beneficial,  with  a 
focus  on  the  underwater  domain.  The  following  sections  will  give  an  overview  of 
the  various  classes  of  underwater  vehicles  and  their  specific  characteristics.  We  will 
also  illustrate  some  of  the  many  applications  for  which  lAIJVb  are  used  today  and 
how  these  applications  shaped  their  characteristics.  The  type  of  IAIJVI  determines 
the  accuracy  of  its  on-board  navigation  sensors,  while  the  application  dictates  the 
required  localization  accuracy. 

1.1.1  Platforms 

Propelled  Vehicles 

The  earliest  and  still  most  common  type  of  IAIJVI  consist  of  a  torpedo  shaped  body 
with  a  single  thruster.  The  diameter  of  larger  models  ( Bluefin21 )  is  often  533  mm 
(2i”)  53,  the  same  as  that  of  a  heavy-weight  torpedo  while  the  smaller  ones  ( Remus 
600,  Bluefin9 )  have  the  same  diameter  as  a  light-weight  torpedo  -  324  mm  (12.75”). 
Their  lengths  vary  between  1  m  and  7  m  and  several  models  consist  of  segments  which 
can  be  added  or  removed  to  adapt  the  payload  section  to  the  mission’s  specific  needs. 
Note  the  transitions  between  the  modules  on  the  Bluefin21  and  Gavia  in  figure  11-21 
The  weight  ranges  between  20  kg  for  the  very  small  and  portable  IVER  (top  left  in 
figure  [17211  and  900  kg  for  a  large  Remus  6000,  but  all  vehicles  are  usually  positively 
buoyant.  This  is  a  security  feature  which  ensures  that  a  vehicle  will  return  to  the 
surface  if  all  power  is  cut.  As  a  result  this  type  of  IAIJVI  needs  to  move  forward  to 
stay  submerged.  The  maximum  achievable  depth  is  6000  m  for  the  large  vehicles  and 
around  100  m  for  the  smaller  ones.  Cruising  speeds  range  between  1-3  m/s.  The 
excellent  hydrodynamic  properties  of  the  long  slender  body  only  require  a  relatively 
low-power  propulsion  for  a  given  cruising  speed  and  payload.  As  a  result,  variations 
of  the  torpedo-shaped  IAIJVI  such  as  the  ones  in  figure  11-21  are  the  vehicle  of  choice 
for  many  applications.  Early  lATJVk  were  usually  large,  but  with  recent  advances  in 
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Figure  1-2:  Various  torpedo-shaped IXlJVk.  Top  left:  the  low-cost,  “human-portable” 
IVER  from  Ocean  Server.  Top  right:  two  Bluefin21  prior  to  being  loaded  onto  the 
Leonardo  during  the  GOATS04  experiment.  Bottom  row:  the  Gavia  IXUVI  made  by 
Hafmynd. 


miniaturization  of  the  key  components  (battery,  propulsion,  navigation  sensors)  sev¬ 
eral  smaller,  “human-portable”,  models  appeared  recently.  Other,  more  specialized 
vehicles  exist  such  aslWHOIfs  Seabed  sza  shown  in  figure  11-41  This  vehicle  is  specif¬ 
ically  designed  for  underwater  imaging.  To  accomplish  this,  it  needs  to  be  able  to 
precisely  control  its  altitude  and  to  change  its  depth  without  forward  motion.  The 
shape  of  this  vehicle,  with  a  large  separation  between  the  center  of  gravity  and  the 
center  of  buoyancy,  provides  a  very  stable  camera  platform.  An  even  more  maneu¬ 
verable  IAUVI  is  the  HA  UV  [91J  shown  in  figure  11-51  Its  eight  thrusters  allow  it  to 
rotate  around  and  move  along  every  axis  in  3D-space.  Its  main  application  is  ship 
hull  inspection  using  a  forward  looking  imaging  sonar  (figure  11-51) .  Another  special¬ 
ized  design  is  Woods  Hole  Oceanographic  Institution  (IWHOID’s  hybrid  IRQV[/lAUVI 
Nereus  which  will  be  able  to  reach  full  ocean  depth  ra. 
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Figure  1-3:  The  Spray  (top)  and  the  X-Ray  glider  (bottom). 


Buoyancy-Driven  Vehicles 

As  the  high  power  consumption  of  the  propulsion  system  limits  the  range  of  propelled 
lAUVk  to  a  few  100  km  [10]  the  need  for  vehicles  capable  of  crossing  an  ocean  led  to 
the  development  of  buoyancy  driven  gliders.  All  gliders  such  as  the  Spray  or  the 
X-Ray ,  both  shown  in  figure  11-31  are  capable  of  changing  their  displaced  volume  to 
become  positively  or  negatively  buoyant  by  pumping  oil  from  an  internal  reservoir 
to  an  outside  bladder.  As  a  result  of  the  buoyancy  change,  the  glider  ascends  or 
descends  within  the  water  column.  A  set  of  “wings”  then  adds  a  forward  component 
to  the  otherwise  purely  vertical  motion.  The  glider  performs  a  sawtooth  pattern 
(figure  15-31)11  which  can  take  it  to  depths  of  more  than  2000  m.  The  internal  battery 
pack  can  be  shifted  along  the  longitudinal  axis  to  provide  pitch  control  as  well  as 
rolled  around  the  longitudinal  axis  to  provide  yaw  control  in  conjunction  with  a 
set  of  vertical  fins.  A  detailed  description  of  Seaglider  can  be  found  in  [30] .  The 
propulsion  system  only  consumes  power  during  the  activation  of  the  pump  on  the 
sea  surface  or  near  the  bottom.  This  type  of  propulsion  is  very  efficient,  requiring 
only  0.5  W  [SD]  averaged  over  the  entire  period  of  operation,  and  enables  transects  of 


28 


Chapter  1.  Introduction 


Figure  1-4:  Jaguar  Seabed’s  sister  ship.  An  I  AT  J  Vi  specifically  developed  for  underwa¬ 
ter  imaging.  Left:  Seabed  without  fairings  showing  the  large  floatation  (yellow)  on  the 
top  segment  and  the  heavy  battery  compartment  in  the  bottom  segment.  The  flash  is 
mounted  at  the  stern  in  a  glass  bowl  and  the  camera  in  the  bow.  This  ensures  max¬ 
imum  separation  between  light  source  and  camera  axis  to  avoid  backscatter.  Right: 
photo  mosaic  created  from  ~100  pictures  taken  by  an  IR.O VI  of  an  iron  age  shipwreck 
off  Ashkelon,  Israel.  Photo  courtesy  of  Hanu  Singh. 


several  1000  km  [031  •  However  it  limits  the  glider  to  speeds  of  0.2-0. 5  m/s,  which  can 
become  a  problem  in  the  presence  of  strong  currents.  The  need  to  keep  the  total  power 
consumption  around  1  W  does  not  allow  for  sophisticated  navigation  equipment  as 
outlined  in  section  12.1.31  A  recently  developed  glider,  the  X-Ray,  shown  in  figure  11-31 
and  described  in  [TJ.  mitigates  some  of  the  disadvantages  of  gliders.  Being  significantly 
larger  than  other  gliders  it  provides  enough  battery  capacity  for  more  sophisticated 
payloads  and  navigation  sensors.  The  X-Ray’s  steeper  ascent  and  descent  angles  cause 
a  forward  speed  of  0.75  to  1.5  m/s  which  is  close  to  the  cruising  speed  of  propelled 

[ATTVk. 

1.1.2  Applications 

Due  to  the  increasing  availability  and  reliability  oflAUVk  they  are  now  used  for  a  wide¬ 
spread  range  of  applications.  The  following  sections  will  illustrate  a  few  examples 
from  the  large  and  growing  number  of  tasks  performed  by  underwater  vehicles  today. 
These  applications  increase  in  complexity  which  requires  an  increasing  level  in  vehicle 
autonomy.  The  examples  also  show  how  the  different  designs  emerged  out  of  task- 
specific  needs.  The  certainty  of  the  vehicle’s  position  estimate  determines  the  quality 
of  the  results  (e.g.  maps)  for  all  applications.  Hence,  these  examples  illustrate  how 
the  utility  of  autonomous  marine  vehicles  is  directly  tied  to  their  ability  to  self-localize 
and  therefore  use  all  available  information  to  minimize  the  position  uncertainty. 

Mapping 

The  earliest  and  still  most  widespread  application  for  lAUVk  is  mapping.  Early  IAUVI 
payloads  would  only  contain  sensors  for  physical  and  biological  water  properties  such 
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as  temperature,  salinity,  turbidity  and  fluorescence,  but  the  increased  payload  capa¬ 
bility  of  today’s  vehicles  allows  for  sonars  which  provide  micro-bathymetry  (pencil  or 
multi-beam)  [71j ,  video- like  acoustic  images  flDIDSQNI  in  figure  11-51)  of  underwater 
structures  (side  scan  figure  ll-bl)  and  even  information  about  buried  objects,  such  as 
mines,  from  low-frequency  sub-bottom  profiling  sonars  [29]. 

More  recently,  cameras  have  been  attached  tolAUVk  The  absence  of  natural  light, 
except  in  very  shallow  waters,  requires  artificial  illumination.  Also,  the  strong  atten¬ 
uation  of  light  underwater  limits  the  distance  between  camera  and  the  photographed 
object,  thus  a  single  picture  can  only  cover  a  few  m2.  To  cover  larger  areas  anlAIJVI 
typically  takes  several  thousand  pictures  during  a  mission.  Sophisticated  mosaicking 
techniques  are  then  used  to  combine  the  individual  frames  to  a  complete  picture  of 
the  seafloor  [80] .  This  technique  has  been  used  to  map  ancient  shipwrecks  [9J  as  well 
as  coral  reefs  j3]. 

Most  mapping  applications  require  the  vehicle  to  run  in  a  pre-programmed  “lawn- 
mower”  pattern  to  ensure  that  the  sensors  cover  a  predefined  area.  The  data  collected 
by  the  sensors  is  stored  on-board.  After  the  mission,  sensor  data  is  combined  with 
the  vehicle’s  navigation  data  in  order  to  create  a  map.  Consequently,  mapping  appli¬ 
cations  require  the  vehicle  to  have  a  very  precise  estimate  of  its  position  throughout 
the  entire  mission  as  the  navigation  data  is  later  used  to  globally  reference  the  sensor 
readings.  This  is  particularly  relevant  for  underwater  imaging  applications. 


Inspection 

A  special  case  of  a  mapping  mission  is  the  inspection  scenario.  Here  the  I  All  VI  is 
required  to  map  one  or  several  features  such  as  oil  rigs,  harbor  structures  or  ship  hulls. 
These  features  are  mostly  man-made  and  have  a  very  complex  shape.  The  inspection 
requires  the  vehicle  to  be  very  close  to  the  feature  so  that  it  can  be  mapped  in 
detail  with  underwater  imaging  or  a  high  resolution  sonar.  Thus  the  vehicle  needs  to 
adapt  its  trajectory  to  the  feature’s  shape  to  provide  full  sensor  coverage.  Figure  [U5] 
shows  IMTT|/Rlnefin’s  Hovering  Autonomous  Underwater  Vehicle  flHAUVI)  mapping 
the  ship  hull  of  a  decommissioned  cruiser.  Future  applications  include  the  inspection 
of  Liquefied  Natural  Gas  fILNGI)  tankers  for  explosives  attached  to  their  hulls  prior  to 
entering  port  areas.  An  explosion  of  a  ship  with  such  cargo  could  have  a  catastrophic 
effect.  Today  these  inspections  are  carried  out  manually,  requiring  a  large  number  of 
divers.  The  IHATJVI  would  approach  the  ship  on  a  preprogrammed  track  and  would 
then  move  in  a  complex  lawnmower  pattern  along  the  hull.  Using  the  forward  looking 
Doppler  Velocity  Log  (1DVLI) .  it  obtains  information  about  its  distance  to  the  hull  as 
well  as  relative  speed.  By  extracting  features  from  the  Dual  Frequency  Identification 
Sonar  flDIDSQNI)  the  IHAIJVI  ensures  that  it  obtains  a  consistent  map  covering  the 
entire  hull  US- 
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Figure  1-5:  Ship  hull  inspection  using  the  \HAUV\  to  inspect  the  Salem  in  Quincy, 
MA,  USA.  Top  left:  the  \HA  UV\  in  the  pool  at  IMITI  Note  the  eight  thruster  for 
increased  maneuverability,  the  forward  looking  IDYLI  (black  cone  with  red  circles) 
which  provides  distance  and  speed  of  the  \HA  UV I  relative  to  the  ship  hull  and  the 
forward  looking  IDIDSONl  to  the  left  of  the  IDVL1  Top  right:  output  of  the  forward 
looking  IDIDSONl  showing  cooling  systems  and  bio  fouling  on  the  hull  of  the  Salem. 
Bottom  row:  the  decommissioned  heavy  cruiser  USS  Salem  ( CA-139)  in  Quincy,  MA, 
USA  while  inspected  by  the  \HA  UV 1  Top  pictures  courtesy  of  Jerome  Vaganay  from 
Bluefin  Robotics. 


Tracking 

Unlike  mapping  applications  in  which  the  area  of  interest  is  usually  static  and  the 
vehicle  trajectory  can  often  be  entirely  preprogrammed,  tracking  applications  require 
a  higher  level  of  vehicle  autonomy.  In  a  typical  tracking  application  the  vehicle,  after 
being  released,  enters  a  search  or  loiter  pattern  until  its  sensors  pick  up  signatures 
of  the  feature  which  is  to  be  tracked.  It  then  breaks  from  its  initial  search/loiter 
behavior  and  adapts  its  trajectory  to  maximize  sensor  effectiveness  and  information 
gain  about  the  feature. 

In  [28j  Eickstedt  et  al.  simulate  an  Anti-Submarine  Warfare  (IASWI)  scenario  in 
which  an  IAIJVI  with  a  towed  array  starts  in  a  loiter  pattern  and  listens  for  acous- 


1.1.  Autonomous  Marine  Vehicles 


31 


Figure  1-6:  Autonomous  pipeline  inspection.  Left:  sidescan  transducers  on  the  Gavia 
I  ATI  VI  Photo  courtesy  of  the  NOAA  OE  Bonaire  2008  Expedition.  Right:  sidescan 
image  of  an  undersea-pipeline  taken  by  a  Gavia  I  AT  J  VI  The  image  shows  echoes  from 
the  raised  pipeline  as  well  as  the  acoustic  shadow  it  casts  on  the  sea  floor.  A  vehicle 
course  that  is  not  parallel  to  the  pipeline  creates  a  distorted  image.  Image  courtesy 
of  Hamynd. 


tic  signatures  of  a  potential  target.  After  picking  up  a  signature  and  obtaining  an 
estimate  of  the  target’s  course  and  speed  it  attempts  to  close  the  distance  between 
itself  and  the  target  and  to  align  its  array  for  optimal  tracking.  Later  experiments 
verified  the  feasibility  of  this  approach.  In  m  German  et  al.  use  the  IAUVI  ABE  to 
find  hydrothermal  vents,  an  often  very  small  feature  (few  m2)  on  the  sea  floor.  ABE 
initially  performs  a  “lawn-mower”  search  over  a  predefined  area.  After  completing  its 
initial  search  it  autonomously  revisits  all  locations  where  chemical  sensors  suggested 
the  presence  of  a  vent  for  a  finer  grid  search.  If  required,  this  process  of  “revisit 
and  refine”  is  repeated  until  the  vents  are  sufficiently  well  localized  to  warrant  a 
small-scale  photo  mosaicing  to  visually  confirm  the  hydrothermal  vent’s  presence. 


Another  tracking  application  is  shown  in  figure  11-61  The  Gavia  IAUVI  is  released 
close  to  an  underwater  pipeline.  While  underway  it  obtains  readings  from  its  side 
scan  sonar  which  it  synthesizes  to  the  picture  shown  on  the  right  side  in  11-61  An 
online  feature  detector  tries  to  extract  the  pipeline  from  the  sonar  image  and  sets 
a  course  which  keeps  the  IAUVI  parallel  to  the  pipeline  at  a  fixed  distance.  This  is 
particularly  difficult  when  the  pipeline  is  partially  buried. 
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1.2  Contributions  of  this  Thesis 

1.2.1  Problem  Statement 

The  previous  section  showed  the  two  reasons  why  almost  all  applications  for  underwa¬ 
ter  vehicles  rely  on  the  vehicle  having  a  very  accurate  estimate  of  its  position.  First, 
the  vehicle  continuously  makes  unsupervised  decisions  based  on  its  position  while  the 
mission  is  carried  out.  Second,  the  utility  of  the  collected  data  is  directly  related  to 
the  precision  with  which  the  samples  can  be  localized  in  a  global  frame. 

For  almost  all  robots  operating  outdoors  -  on  land  and  in  the  air  -  the  localization 
problem  has  been  resolved  with  the  advent  of  IGPS1  A  very  affordable  receiver  is 
able  to  provide  an  absolute  position  at  a  high  rate  leading  to  estimates  which  are 
accurate  to  a  few  meters.  Using  additional  infrastructure  such  as  differential  IGPSI 
the  accuracy  can  be  increased  to  a  few  centimeters.  Robots  operating  indoors  usually 
do  not  have  access  to  the  IGPSl  signal  but  modern  sensors,  such  as  laser  scanners  and 
high  resolution  cameras  can  extract  a  rich  set  of  features  which  can  greatly  alleviate 
the  localization  problem. 

This  leaves  underwater  locations  as  the  single  largest  domain  excluded  from  to¬ 
day’s  most  prevalent  localization  techniques.  While  underwater  sensors  continue  to 
improve,  the  strong  absorption  of  almost  the  entire  Radio-Frequency  (1RFI)  spectrum 
in  salt  water  will  impose  physical  limitations  on  radio-based  localization  methods  for 
the  foreseeable  future,  just  as  generally  feature-poor  marine  environments  will  limit 
the  usability  of  natural  feature  based  navigation  methods.  In  the  absence  of  these 
classic  options  new  strategies  such  as  cooperation  for  navigation  will  play  an  impor¬ 
tant  role  in  ensuring  that  the  navigation  accuracy  for  underwater  vehicles  will  be 
similar  to  that  which  has  become  standard  for  outdoor  robots. 

The  particular  strength  of  cooperative  navigation  is  the  fact  that  it  does  not  re¬ 
quire  any  additional  infrastructure  or  even  instrumentation  of  the  vehicle.  The  sensor 
and  communication  package  which  is  standard  on  today’s  underwater  vehicles  is  suf¬ 
ficient  and  adding  cooperative  navigation  requires  merely  a  change  in  the  vehicle’s 
navigation  and  control  software.  The  main  requirement  for  this  approach  -  deploying 
more  then  a  single  vehicle  -  will  be  satisfied  for  most  of  the  upcoming  deployments 
due  through  the  increased  reliability  and  availability  of  suitable  platforms. 

This  thesis  investigates  the  main  shortcomings  of  conventional  (non-cooperating) 
vehicle  navigation.  It  identifies  and  proposes  solutions  for  the  three  key  problems 
which  need  to  be  resolved  to  successfully  implement  cooperative  navigation. 

1.2.2  Cooperative  Localization  Algorithm 

The  methods  previously  proposed  for  cooperative  localization  of  land  and  air  vehicles 
make  assumptions,  particularly  on  the  reliability  and  bandwidth  of  the  communi¬ 
cations  channel  which  do  not  hold  underwater.  We  propose  an  algorithm  which  is 
specifically  adapted  to  the  underwater  communication  and  messaging  infrastructure 
as  well  as  to  the  vehicle’s  sensor  suites  to  provide  a  robust  estimate  of  the  actual 
position. 


1.3.  Thesis  Outline 


33 


1.2.3  Maintaining  Consistency 

If  a  number  of  vehicles  exchange  navigation  information,  the  position  estimates  of 
the  participating  vehicles  will  become  correlated  and  will,  over  time,  suffer  from 
overconfidence.  This  is  independent  of  the  cooperative  localization  algorithm  used 
and  must  be  mitigated  as  overconfidence  in  the  vehicle’s  position  estimate  can  lead 
to  catastrophic  results.  One  option  is  the  use  of  dedicated  navigation  beacons  as  this 
method  ensures  that  the  flow  of  navigation  information  is  uni-directional.  For  the 
more  general  case  in  which  each  vehicle  participates  in  cooperative  navigation  actively, 
by  broadcasting,  or  passively,  by  receiving,  we  propose  an  algorithm  which  selectively 
incorporates  information  while  keeping  the  positions  of  all  vehicles  decorrelated.  This 
selective  update  algorithm  works  with  any  underlying  navigation  algorithm. 

1.2.4  Motion  Planning  for  Cooperating  AUVs 

The  amount  by  which  we  can  decrease  the  uncertainty  in  our  position  estimate 
through  cooperation  depends  not  only  on  the  certainty  in  the  position  estimate  of 
our  cooperation  partner,  but  also  on  the  geometry  between  the  vehicles.  A  special 
case  of  cooperative  navigation  uses  vehicles  which  are  dedicated  navigation  beacons. 
As  the  only  purpose  of  these  vehicles  is  to  collectively  minimize  the  position  uncer¬ 
tainty  of  the  receiving  vehicles  they  need  to  constantly  adapt  their  position.  We 
propose  a  distributed  algorithm  which  dynamically  positions  each  of  these  designated 
beacon  vehicles  to  maximize  the  effect  of  their  position  broadcasts. 

1.2.5  Experiments 

We  first  tested  the  performance  of  the  communication  infrastructure  used  in  order  to 
assess  how  well  it  is  suited  for  cooperative  navigation.  We  then  carried  out  a  series  of 
experiments  to  verify  the  performance  and  robustness  of  our  cooperative  navigation 
algorithm.  By  first  relying  on  surface  vehicles  only,  which  had  access  to  IGPS1  but 
would  only  communicate  through  the  acoustic  channel  available  to  submerged  lAUVb. 
we  obtained  ground-truth  which  the  results  of  our  localization  algorithm  could  be 
compared  against.  We  then  substituted  one  of  the  surface  craft  with  an  [AUV|  featur¬ 
ing  a  very  sophisticated  navigation  suite  and,  in  another  experiment,  with  a  glider 
possessing  very  minimalist  sensors.  Both  experiments  showed  that  the  cooperation 
would  lead  to  a  noticeable  reduction  in  the  position  uncertainty. 


1.3  Thesis  Outline 

The  remainder  of  the  thesis  is  organized  as  follows: 

Chapter  lAUVb:  Communication  and  Navigation  Capabilities 

We  start  by  giving  an  overview  of  the  state  of  the  art  in  the  two  domains 
which  are  most  crucial  to  cooperative  navigation:  underwater  communication 
and  vehicle  navigation  sensors.  The  most  commonly  used  navigation  sensors, 
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along  with  their  relevant  performance  parameters,  are  introduced  as  well  as  the 
options  for  underwater  communication. 

Chapter  [3t  Cooperative  Localization 

This  chapter  establishes  the  probabilistic  framework  in  which  we  will  present  the 
cooperative  localization  problem.  We  introduce  the  two  classical  methods  which 
have  been  used  for  localization  problems  in  general  and  the  related  work  in  which 
they  have  been  adapted  for  cooperative  localization.  Based  on  the  shortcomings 
of  these  two  methods  we  propose  our  algorithm.  This  chapter  also  addresses 
the  problem  of  correlations  arising  from  sharing  navigation  information  and 
the  resulting  overconfidence  in  the  position  estimate.  Our  method,  the  IIUI 
algorithm,  is  a  general  solution  for  all  cooperating  navigation  algorithms  which 
modifies  their  update  step  to  keep  all  vehicle  positions  decorrelated  and  thereby 
preventing  overconfidence. 

Chapter  0:  Intra- Vehicle  Geometries  for  Cooperating  lAUVb 

The  specific  scenario,  in  which  some  vehicles  serve  as  dedicated  navigation  bea¬ 
cons,  requires  that  these  vehicles  adapt  their  position  in  order  to  maintain  an 
advantageous  relative  position  to  all  other  vehicles  for  which  they  provide  the 
information  for.  We  show  how  the  geometry  between  beacon  and  receiving  ve¬ 
hicles  has  a  strong  influence  and  we  propose  an  algorithm  which  positions  the 
beacon  vehicles  optimally  based  on  locally  available  information. 

Chapter  [5J  Experiments 

This  chapter  shows  the  results  of  the  experiments  we  carried  out  to  validate 
our  cooperative  navigation  algorithm  using  different  surface  and  underwater 
platforms.  We  also  compare  the  performance  of  our  algorithm  with  the  classical 
approaches. 

Chapter  0  Conclusion 

The  last  chapter  shows  the  direction  for  future  research  and  summarizes  the 
contribution  of  this  thesis. 

Appendix  lAt  Coordinate  Systems 

The  appendix  establishes  the  coordinate  system  used  throughout  the  thesis. 


Chapter  2 


lAUVIs:  Communication  and 
Navigation  Capabilities 

2.1  Underwater  Navigation 

2.1.1  Navigation  Sensors 

This  section  gives  an  overview  of  the  sensors  commonly  used  in  underwater  vehicles. 
It  outlines  their  particular  characteristics  and  shows  how  several  sensors  are  used 
jointly  to  determine  a  vehicle’s  position.  It  is  important  to  note  that  our  cooperative 
navigation  approach  does  not  replace  any  of  these  instruments,  but  adds  a  “virtual” 
sensor  by  combining  the  measurements  of  the  physical  sensors  from  its  own  and  other 
vehicles. 


Depth  Sensor 

All  submersible  vehicles  are  outfitted  with  a  pressure  sensor  which  allows  them  to 
determine  their  absolute  depth  with  high  accuracy  and  a  high  update  rate.  As  a  result 
all  other  underwater  navigation  systems  are  only  used  to  resolve  the  2D  position,  (i.e. 
longitude  and  latitude)  and  all  underwater  vehicle  related  localization  problems  are 
stated  in  2D. 


Magnetic  Compass 

Like  the  pressure  sensor,  a  compass  is  part  of  the  basic  navigation  sensor  suite  of 
every  underwater  vehicle  as  it  is  an  inexpensive  and  low-power  device.  It  provides 
the  3D- vector  of  the  local  magnetic  field.  Before  computing  the  heading  from  the 
magnetic  field  vector  it  is  necessary  to  carefully  calibrate  the  compass  each  time  the 
vehicle’s  area  of  operation  changes,  as  the  difference  between  the  orientation  of  the  3D 
magnetic  field  vector  and  the  direction  of  true  north  (called  “variation” )  depends  on 
the  geographic  location.  In  addition  to  the  spatially  slow  variation,  there  are  highly 
localized  “magnetic  anomalies” .  The  compass  output  is  also  affected  by  its  position 
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in  the  vehicle  as  electrical  currents  create  magnetic  fields  which  cannot  be  discerned 
from  the  Earth’s  magnetic  field. 

Global  Positioning  System  (IGPSh 

The  IGPSI  is  able  to  provide  absolute  position  information  for  outdoor  land  robots  as 
well  as  Unmanned  Aerial  Vehicles  (lUAVk).  but  the  strong  absorption  of  electromag¬ 
netic  waves  by  sea- water  prohibits  the  use  of  thelGPSlbv  submerged  lAIJVIs .  Nonethe¬ 
less,  almost  all  underwater  vehicles  today  are  equipped  with  a  IGPSI  receiver  as  it  can 
be  used  to  get  a  position  fix  before  the  start  of  the  mission  or  during  intermittent 
surfacings. 

Flow  meter 

A  flow  meter  consists  of  a  tube,  usually  mounted  in  line  with  the  main  vehicle  axis 
and  is  open  to  the  surrounding  water  on  both  sites.  It  contains  a  propeller  which  is 
spun  by  the  water  running  through  the  tube  as  the  vehicle  moves.  A  sensor  attached 
to  the  propeller  determines  the  rotational  speed  which  can  be  converted  into  flow 
speed  and  thereby  giving  an  indication  of  the  vehicle’s  speed  relative  to  the  water 
body. 

Beacon  Techniques 

The  most  commonly  used  way  to  obtain  absolute  position  information  underwater 
is  through  the  use  of  beacons.  These  beacons  are  at  known  locations  and  the  IAIJVI 
obtains  the  range  and/or  bearing  to  several  of  these  and  then  calculates  its  position 
through  trilateration  or  triangulation.  Based  on  the  location  of  the  transceivers  we 
can  identify  three  different  baseline  systems. 

Standard  Long  Baseline  (1LBLI) :  A  tvpicallLBLlconfiguration  is  shown  in  figurel2-lal 
Two  or  more  beacons  are  deployed  around  the  perimeter  of  the  area  in  which 
the  IAIJVI  will  operate.  These  beacons  are  anchored  and  float  on  the  surface 
or,  particularly  in  deeper  water,  a  few  meters  above  the  sea  floor.  Each  unit 
listens  to  acoustic  query  pings  on  a  common  receive  channel.  After  receiving 
a  query  ping  from  an  IAIJVI  each  unit  waits  for  a  unique-specific  Turn-Around 
Time  (ITATI)  tTAT  and  then  sends  out  a  reply  ping  on  its  individual  transmit 
channel.  The  IAIJVI  then  receives  the  reply  pings.  The  transmit  channel  as  well 
as  thelTATlare  different  for  each  unit.  A  unique  ITATI  ensures  that  two  beacons 
will  not  interfere  by  transmitting  at  the  same  time  and  by  using  different  trans¬ 
mit  frequencies  the  beacons  provide  a  way  for  the  IAIJVI  to  identify  from  which 
unit  a  reply  ping  was  sent.  The  time  difference  At*  between  sending  out  the 
query  ping  and  receiving  a  reply  can  then  be  used  to  determine  the  One-Way 
Travel  Time  (IQWTTjl  t°wtt. 
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(a)  Standard  |LBL]  (b)  Synced  |LBL] 


Figure  2-1:  Beacon-based  underwater  localization  techniques. 


The  distance  between  a  beacon  i  and  the  AUV  is  then  given  by 

1  =  _c_ 

^  fOWtt 

Li 

where  the  speed  of  sound  c  is  either  a  pre-programmed  value  or  measured  on¬ 
board.  Using  range  measurements  to  several  beacons  and  the  beacon  positions 
stored  in  the  vehicle  before  deployment,  the  lAUVl  can  now  trilaterate  its  posi¬ 
tion. 

The  maximum  possible  distance  between  the  lAUVl  and  a  beacon  as  well  as  the 
localization  accuracy  depend  on  the  the  frequency  band  used  for  query  and  reply 
pings.  Long-range  ILBLlsvstems  using  the  12  kHz  band  work  over  distances  as 
long  as  10  km  [Q3j  and  can  provide  provide  an  absolute  position  with  an  error 
between  1  m  and  10  m.  Short-range  ILBLI  systems  using  frequencies  around 
300  kHz  band  can  achieve  sub-centimeter  precision,  but  the  maximum  range 
is  limited  to  100  m  [03].  The  indicated  errors  assume  that  large  outliers  have 
been  filtered  out.  These  outliers,  which  can  be  seen  in  figure  12-21  are  due 
to  reflections  from  temperature  and  salinity  discontinuities  which  are  further 
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Mission  time  [s] 


Figure  2-2:  Time-of-flight  obtained  from  four  LBL  beacons.  The  plot  shows  significant 
outliers  for  all  beacons,  particularly  between  400  s  and  600  s.  This  data  was  obtained 
during  the  GOATS2002  experiment. 


explained  in  section  12.2.21 

ITBLl  variants:  Standard  ILBLI  systems  such  as  the  one  described  above  are  not  well 
suited  for  large  groups  oflAUVk  because  only  one  vehicle  at  a  time  can  query  the 
beacon  network  and  get  a  position  update.  Thus  the  position  update  interval 
increases  with  the  number  of  vehicles.  Newer  [LBL]  systems,  like  the  one  recently 
developed  by  A CSA  [83j  and  shown  in  figure  12-lbl  have  synchronized  clocks  in 
the  beacons  and  the  IAUVI  transceiver  units.  The  beacons  broadcast  a  ping 
containing  a  unique  identifier  at  fixed  time  intervals.  When  the  IAUVI  receives 
this  ping,  the  beacon’s  known  broadcast  schedule  and  the  synchronized  clock’s 
time  ensure  that  the  vehicle  knows  when  a  ping  was  sent  and  can  directly 
compute  the  IQWTTI  The  synchronized  clocks  thereby  eliminate  the  need  for 
query  pings  and  allow  all  vehicles  within  range  of  the  beacons  to  get  a  range 
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(b)  R.EMI  JS I  AT  J  VI  with  up-(arrow)  and  downward  looking  fDVLI 


(a)  |DVL] 


Figure  2-3 


to  the  broadcasting  beacon.  As  a  result  the  ping  interval  is  independent  of  the 
number  of  vehicles  relying  on  the  beacon  network. 

Another  improvement  over  conventional  ILBLI  is  the  system  depicted  in  fig¬ 
ure  12-lcl  Building  on  the  setup  in  figure  12-lbl  the  beacons  now  transmit  their 
GPS  position  along  with  the  unique  identifier.  As  with  the  system  described 
previously,  the  vehicles  do  not  need  to  query  the  beacons.  With  the  position 
of  the  beacons  embedded  in  the  ping  the  beacons  can  float  freely  and  it  is  not 
necessary  to  store  their  coordinates  in  the  lAUVl  before  deployment. 

Ultra-Short  Baseline  (lUSBLh:  Another  variant  of  beacon  based  navigation  sys¬ 
tems  is IUSBLI  (figure l2Udl) .  Here  the  beacon  is  of  the  same  kind  as  in  a  standard 
ILBLI  system,  but  the  transceiver  on  the  lAUVl  contains  several  receiving  elements 
which  are  very  close  to  each  other.  After  querying  the  beacons  the  reply  ping 
is  captured  by  all  receiving  elements.  The  phase  difference  between  the  signals 
coming  from  the  different  receiving  elements  allows  the  lAUVl  to  compute  a  bear¬ 
ing  to  the  beacon.  Combined  with  the  beacon  position  stored  in  the  lAUVl  and 
the  distance  d  obtained  from  the  IQWTTI  the  vehicle  can  compute  its  absolute 
position  using  only  a  reply  from  a  single  beacon. 

Modern  beacon-based  systems  such  as  the  ones  shown  in  figures  12-lbl  12-lcl  and 
12-  ldl  significantly  decrease  the  pre-deployment  effort  when  compared  to  early  beacon- 
based  systems  such  as  the  standard  ILBLI  However  all  beacon-based  systems  confine 
the  operating  area  of  the  vehicles  to  a  polygon  of  beacons  or,  as  in  the  case  oflTJSBLl 
to  the  coverage  radius  of  a  single  beacon.  Thus  beacon-based  navigation  is  only 
feasible  for  operating  areas  of  0(10  km2)  in  size. 

Doppler  Velocity  Log  flDVLI) 

A IDVLI  (figure  !2-3al)  is  a  device  which  typically  has  four  transceiver  units  that  emit 
acoustic  pulses.  When  a  IDVLI  is  used  for  navigation  purposes  it  is  usually  mounted  to 
a  vehicle  such  that  the  transceivers  are  facing  downward.  If  the  IDVLI  is  close  enough 
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to  the  bottom,  the  transceiver  will  receive  the  reflected  pulses  ( “bottom- lock” )  and 
as  the  transceivers  are  mounted  at  an  angle  with  respect  to  the  sea  floor  plane,  the 
received  pulses  will  be  subject  to  a  Doppler-shift  if  the  vehicle  is  moving.  Combining 
the  measured  Doppler- shifts  from  all  4  sensors  with  the  built-in  roll,  pitch  and  heading 
sensors  the  IDVLI  can  then  compute  the  vehicle’s  3D-speed  vector  vv  =  [x,  y,  z]  in  a 
world-referenced  frame. 

The  maximum  distance  between  the  IDVTdun.lt  and  the  sea  floor  depends  on  the 
operating  frequency  of  the  transceivers.  Low-frequency  (150  kHz)  IDVLI  can  obtain 
bottom-lock  for  ranges  up  to  500  m,  while  a  high-frequency  [DVL]  (1200  kHz)  requires 
less  than  30  m. 

The  ranges  indicated  above  can  only  be  obtained  under  ideal  conditions.  A  soft 
sea  floor  or  vegetation  can  absorb  most  of  the  energy  of  the  incoming  pulse  and 
thereby  significantly  decrease  the  maximum  range.  Another  option  is  to  mount  the 
IDVLI  in  an  upward  looking  configuration  such  that  the  acoustic  pulses  are  reflected 
at  the  water/air  interface  (“surface-lock”).  Then,  the  vehicle  measures  its  speed 
relative  to  the  water  surface,  but  this  strategy  may  introduce  errors  in  the  case  of 
significant  surface  currents.  Figure  !2-3bl  shows  a  REMUS  1004 A II VI  with  a  double- 
IDVLI  configuration.  If  bottom-lock  cannot  be  obtained  with  the  downward-looking 
IDVLI  the  vehicle  tries  to  determine  its  speed  using  the  upward  looking  unit.  Recent 
developments  greatly  increased  the  accuracy  of  IDVLlsvstems  and  errors  as  low  as 
0.2  %  (1200  kHz)  or  1  %  (150  kHz)  can  be  obtained. 


Attitude  Heading  Rate  Sensor  (IAHRSI) 

An  IAHRSI  unit  typically  consists  of  a  3-axis  linear  acceleration  sensor  as  well  as  a 
3-axis  gyroscope  and  a  heading  sensor  (magnetic  compass).  Combining  the  measure¬ 
ments  from  these  sensors,  the  IAHRSI  can  compute  the  3  linear  and  3  angular  velocities 
and  accelerations  (rates)  as  well  as  the  attitude  and  heading  from  the  windowed- 
average  of  the  linear  acceleration  sensor  readings  and  the  compass. 


Inertial  Navigation  System  (1INSI) 

The  sensors  of  an  IINSI  are  the  same  as  those  of  the  IAHRSI  described  above.  In  addition 
to  the  IAHR  SI  the  IINSI  uses  information  from  absolute  position  sensors  (such  as  IGPSI 
or  ILBLI)  and  integrates  the  rate  sensor  readings  to  compute  the  actual  position.  This 
process  is  called  Dead- Reckoning  flDRl .  As  the  linear  and  angular  acceleration  sensors 
are  subject  to  noise,  the  position  derived  from  these  sensors  in  the  absence  of  IGPSI  or 
ILBLI  is  subject  to  a  cumulative  error  and  the  obtained  position  will  drift  with  respect 
to  the  true  position.  The  drift  (error)  e  between  the  vehicle’s  true  position  xtrUe  and 
the  position  obtained  with  IDR.I  x DR  are  expressed  as  “drift  over  time”  or  “drift  over 
distance  traveled” 


e  = 
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Typically  the  heading  and  rate  sensors  of  an  ANSI  are  less  noisy  than  those  of  a 
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comparably  cheap  lAHRSI  which  decreases  the  problem  of  accumulated  drift.  An IINSI 
which  fits  into  the  hull  of  an lAIJVl shows  typical  drift  rates  of  1  km/h  [46].  The  exact 
performance  of  the  most  precise  UNSI  available,  the  ones  found  in  nuclear  submarines, 
remains  classified,  but  is  expected  to  be  0(0.01  km/h). 


2.1.2  Sensor  Fusion 

The  sensors  described  inl2.1.1lcan  be  divided  into  three  groups. 


1.  Absolute  position  sensors 

The  output  of  these  sensors  is  the  absolute  position  of  the  vehicle  in  a  global 
reference  frame  x  =  [x.  y.  zjlGPSI  and  ILBLI  are  examples  of  absolute  position 
sensors.  Visually  Augmented  Navigation  (IVAN!)  is  a  method  presented  by  Eu- 
stice  eh  He  shows  that  an  IAUVI  continuously  taking  pictures  of  the  sea  floor 
while  traveling  can  use  these  pictures  to  bound  the  navigation  error  introduced 
by  the  IDR1  sensors.  As  a  result  this  method  can  be  considered  an  absolute  po¬ 
sition  sensor.  Walter  uses  a  similar  technique  to  bound  the  navigation  error  by 
using  the  output  of  an  imaging  sonar  navigating  along  a  ship  hull  for  inspec¬ 
tion  ffl.  A  special  case  of  an  absolute  position  sensor  is  the  pressure  sensor  as 
it  resolves  the  position  of  an  underwater  vehicle  for  the  ^-dimension  only. 


2.  IDRlsensors 

The  output  of  these  sensors  is  a  speed  vector  uv 
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3.  Attitude  and  heading  sensors 

Compass  or  Fiber-Optic  Gyroscopes  (IFOGk)  provide  the  heading/yaw  of  our 
vehicle.  Combining  this  information  with  the  3D  gravity  vector,  which  is  pro¬ 
vided  by  the  IAHR.S1  we  obtain  the  orientation  of  the  vehicle  in  space  described 
by  the  yaw  6*,  pitch  if  and  roll  <f>  angle  (see  appendix  [A]  for  the  definition  of  the 
axes  and  angles). 


The  Main  Vehicle  Computer  (IMVCI)  collects  all  incoming  information  from  the 
sensors  and  constantly  computes  the  pose,  the  position  and  attitude  of  the  vehicle. 
This  process  is  called  sensor  fusion  and  is  the  essential  part  of  every  navigation  al¬ 
gorithm.  The  following  section  describes  a  basic  IDRI  algorithm.  More  sophisticated 
algorithms  are  introduced  in  section  13.41  The  basic  IDRI  algorithm  is  shown  in  algo¬ 
rithm  H]  and  explained  below. 


Initialization 

Before  being  able  to  rely  on  IDRIsensors  the  navigation  algorithm  needs  to  be  initial¬ 
ized  with  an  absolute  position  Xq  from  an  appropriate  sensor  (line  SD .  Usually  this  is 
aIGPSI  which  is  used  on  the  surface  before  the  vehicle  submerges. 
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repeat 

wait 

until  Xq  is  available 

X(t)  =  Xq 

loop  {for  each  dr} 
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9:  if  absolute  position  measurement 

^abs1 

't)  is  availab 

10:  •^abs(^) 

11:  end  if 

12:  end  loop 


Algorithm  1:  Simple  iDRl  which  uses  an  initial  position  x0  and  acceleration  and  speed 
measurements  to  estimate  the  vehicles’s  actual  position  x(t). 


Dead  Reckoning 

Acceleration  sensors  such  as  an  IAHRSI  provide  a  vehicle  referenced  acceleration  vec- 
torn  Oahrs  D  which  can  be  integrated  to  obtain  the  vehicle-referenced  velocity  vector 
(line  EJ)  ■ 

/t+dr 

aAHRs(^)^r 

Other  sensors  such  as  the  IDVLI  or  the  flow  meter  directly  provide  the  speed  vector 
uv (t)  in  vehicle  coordinates.  We  combine  the  speed  vector  w{(VL(t),  with  the  speed 
vector  from  the  IAHR  SI  u](HK  s  (t)  computed  in  the  previous  step.  The  information  from 
the  attitude  sensors  is  then  used  to  compute  R,  the  rotation  matrix  which  converts  the 
joint  vehicle-referenced  velocity  vector  to  a  world-referenced  velocity  vector  (line  [7]). 

u(t)  =  R  ■  uv (t)  =  R  ■  (u\nRS(t)  +  «dvlW) 

Integrating  the  world-referenced  velocity  vector  combined  with  the  initial  position 
leads  to  the  dead-reckoned  position.  Usually  only  the  x  and  ^-coordinate  are  obtained 
using  dead-reckoning,  and  the  ^-component  (depth)  is  readily  available  from  the  pres¬ 
sure  sensor  (line  [HJ)  • 
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Reinitialization 

Whenever  information  from  an  absolute  position  sensor  ccab s(t)  is  available,  the  nav¬ 
igation  algorithm  sets  the  current  position  to  the  obtained  position  ( x(t )  =  £Cabs(t))- 
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Cost,  position  drift,  power  consumption  of  navigation  suite 


Figure  2-4:  Navigation  accuracy,  power  consumption  and  price  of  various  AUV  sensor 
suites  (bottom  to  top):  1  Glider  with  compass  and  attitude  sensor  2  Low-cost  AUV 
with  compass,  attitude  sensor  and  flow  meter  3  Medium-range  AUV  with  IINS1  IDVhl 
and  fURLl  4  High-end  lATTVl  with  EMbased  MB  ITWLl  and  iLRLl  5  Special-task  lATTVl 
with  with HHS1  lUVLl and  IVANl 

*  Drift  in  mid  water-column  when  IDVLI  cannot  obtain  bottom  or  surface  lock. 

**  Assuming  a  10  %  duty  cycle  during  which  the  navigation  sensors  are  powered. 

***  Assuming  that  the  vehicle  was  close  enough  to  the  sea  floor  throughout  the  entire 
mission  to  take  pictures  and  revisit  places. 


2.1.3  State-of-the-Art  in  Underwater  Vehicle  Navigation 


Every  underwater  vehicle  contains  a  subset  of  the  navigation  sensors  described  in 
12.1.11  Which  sensors  are  used  depends  on  the  navigation  accuracy  required  for  the 
mission  as  well  as  the  available  power,  space  and  the  cost  constraints.  Figure  12-41 
shows  five  typical  configurations. 
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Glider  with  very  low  power  sensor  suite 

Gliders  must  operate  for  extended  periods  of  time  without  being  able  to  recharge  their 
batteries.  As  a  result  power  consumption  is  the  limiting  factor  for  the  selection  of 
navigation  sensors,  and  the  navigation  suites  of  a  glider  usually  consists  of  aIGPSI  an 
IAHRSI  and  a  pressure  gauge.  While  submerged  the  glider  uses  the  IAHRSI  combined 
with  a  vehicle  model  to  estimate  its  heading  and  forward  velocity  and  dead-reckon  its 
position.  The  high  noise  and  the  unobservable  variables  in  the  vehicle  model  lead  to 
a  very  high  drift  of  30  %  or  even  more  if  strong  currents  are  present.  On  the  surface 
the  vehicle  resets  its  position  estimate  using  IGPS1 

Low-cost  AUV  sensor  suite 

Low-cost  I  At  J  VI;  such  as  the  IVER  use  a  flow  meter  to  obtain  a  measurement  of  their 
forward  speed  v% .  This  information  combined  with  an  IAHRSI  leads  to  a  significant 
improvement  of  the  navigation  accuracy  when  compared  to  that  of  a  glider. 

Standard  IAUVI  sensor  suite 

The  standard  IAUVI  adds  a  IDVLI  to  the  list  of  sensors.  When  the  IDVLI  is  able  to 
obtain  bottom  lock  a  very  accurate  vehicle-referenced  velocity  vector  uv  is  available 
and  the  navigation  accuracy  improves  by  an  order  of  magnitude.  Drift  rates  as  low  as 
1  %  of  the  distance  traveled  can  be  obtained  with  a  well  calibrated  magnetic  compass. 
Standard  IAU  Vk  operating  in  a  confined  area  are  often  outfitted  with  anILBLI  system. 
When  operating  within  the  polygon  established  by  the  position  of  the  ILBLI  beacons, 
the  position  drift  will  remain  bounded. 

High-end  fAUVl 

The  dominant  source  of  error  in  the  standard EHVl sensor  suite  described  above,  is  in¬ 
troduced  during  the  transformation  of  vehicle-referenced  velocities  to  world-referenced 
velocities  as  a  result  of  errors  in  the  heading  measurements.  Replacing  the  simple 
magnetic  compass  with  a  IFQGI  improves  the  navigation  by  two  orders  of  magnitude 
(0.1  %  of  the  distance  traveled).  When  the  IDVLl  is  not  able  to  obtain  bottom  lock 
all  of  the  vehicles  described  so  far  can  only  rely  on  the  vehicle’s  linear  acceleration 
sensors  to  obtain  velocities.  Due  to  the  large  noise  introduced  by  these  sensors  the 
navigation  accuracy  decreases  dramatically. 

Special-task  IAUVI  using  IVANI 

The  special-task  IAUVI  has  the  same  sensors  as  the  standard  IAUVI  Additionally,  it 
uses  a  bottom- looking  camera  to  take  a  series  of  pictures  of  the  sea-floor.  When 
revisiting  a  point  it  has  taken  a  picture  of  before,  it  is  able  to  recognize  that  fact  and 
the  navigation  algorithm  is  able  to  reset  the  drift.  As  a  result  the  drift  is  bounded. 
This  technique  has  been  described  by  Eustice  in  |31j .  Figure [T5] shows  two  consecutive 
camera  images  with  corresponding  features.  This  method  however  requires  the  lATJVl 
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Figure  2-5:  Two  consecutive  pictures  after  being  processed  by  the  IVANI  algorithm. 
From  the  several  hundred  features  identified  in  each  picture,  only  nine,  marked  by 
the  colored  dots  have  correspondences  in  both  pictures  and  fit  within  the  epipolar 
constraints.  Figures  courtesy  of  Ryan  Eustice. 


to  revisit  points  and  stay  close  enough  to  the  sea  floor  (less  than  10  m)  to  acquire  the 
pictures. 


2.2  Underwater  Communication 

2.2.1  Technologies 

Communication  is  vital  for  any  collaborative  effort  such  as  cooperative  navigation. 
This  section  briefly  describes  communication  based  on  electromagnetic  waves  fIRFIand 
optical) ,  the  most  common  mode  for  untethered  data  exchange  for  land  and  air  based 
systems.  As  their  applicability  under  water  is  limited  to  small  niches,  the  second  part 
of  this  section  addresses  acoustic  communication  the  most  commonly  used  channel 
under  water. 

Radio  Communication 

Using  electromagnetic  waves,  particularly  in  the  Ultra-High  Frequency  (IUHFI)  spec¬ 
trum  around  900  MHz  or  2.4  GHz,  is  the  most  common  way  of  wireless  communication 
for  land  robots  andlUAVk  Radio  communication  enables  these  vehicles  to  communi¬ 
cate  over  distances  from  a  few  meters  to  anywhere  on  the  Earth,  when  using  satellite 
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communication,  at  comparably  high  speeds  (kbps  to  Mbps). 

Due  to  the  strong  attenuation  of  radio  waves  under  water,  radio  communication 
is  only  used  in  two  niches  in  the  realm  of  submersible  vehicles. 

1.  Super  Low  Frequency  flSLFjl  for  long-range  communication 

The  US- American  Seafarer  and  the  Russian  ZEVS  system  each  consist  of  a 
single  base  station,  transmitting  at  76  Hz  and  82  Hz  respectively.  The  antenna 
structures  of  the  base  stations  are  up  to  90  km  [6.1]  in  size  while  the  submarines 
tow  a  very  long  antenna.  This  setup  allows  one-way  communication  from  the 
shore  station  to  submerged  submarines  anywhere  in  the  world  with  data  rates 
(!)  (bits/ min).  Due  to  the  size  and  power  requirements  of  the  transmitter,  the 
submarines  cannot  accommodate  these.  Due  to  the  low  data  rate  provided  by 
this  communication  channel  it  is  only  used  to  signal  a  submarine  to  come  to 
the  surface  to  initiate  communication  through  a  satellite. 

2.  Low  Frequency  flLFh  for  short  range  communication 

Recently,  several  transceivers  have  been  developed  which  operate  in  the  ILF1 
band  (30  kHz  to  300  kHz)  and  can  be  used  for  two- wav  I  AT  J  VI  communication . 
Schill  et  al.  developed  a  transceiver  which  is  small  enough  to  fit  into  a  very  small 
lAUVIand  is  capable  of  data  rates  up  to  8  kbps  and  communication  ranges  up  to 
10  m  |39| .  Rhodes  et  al.  developed  a  similar  system  which  is  now  commercially 
available  and  suitable  for  mid-size  I  Al  J  VI;  [92] . 

While  ISLFI  communication  is  not  feasible  for  lAUVlcommunication  the  ILFlband  is 
a  viable  option  for  short  range  (<10  m)  and  medium  data  rates  (~30  kbps).  Unlike 
optical  communication  it  does  not  require  the  two  transceivers  to  be  aligned  and  it 
is  not  as  bandwidth-constrained  and  susceptible  to  background  noise  as  acoustical 
communication. 

Optical  communication 

Like  radio  communication,  electro-magnetic  waves  in  the  visible  spectrum  are  strongly 
attenuated  under  water.  Additionally,  scattering  from  suspended  particles  further  de¬ 
creases  the  maximum  possible  range.  Schill  [39]  and  Vasilescu  [89]  both  implemented 
a  low-cost  optical  modem  using  powerful  LEDs  as  transmitters  and  photo-diodes, 
achieving  data  rates  around  50  kbps  over  distances  of  3  m.  If  the  two  transceivers 
can  be  perfectly  aligned,  the  maximum  distance  can  be  increased  up  to  10  m  by 
adding  focusing  lenses  to  the  transmit  LED. 

By  using  a  steered  laser  beam  as  a  transmitter  and  a  photo-multiplier  as  a  receiver 
Farr  et  al.  [33]  hope  to  increase  the  distance  to  100  m  and  the  transmission  rate  to 
10  Mbps. 

Acoustic  Communication 

Given  the  range  restrictions  of  electromagnetic  waves  in  the  optical  and  RF  spec¬ 
trum,  acoustic  communication  is  the  only  available  technology  today  for  underwater 
communication  over  longer  ranges. 
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Figure  2-6:  The  IWHOT1  micromodem.  Left:  the  base  board  and  an  additional  IDSPI 
board  to  process  high  data-rate  message.  Right:  the  transducer  (black  cylinder) 
mounted  into  a  towfish. 


2.2.2  Acoustic  Communication  Constraints  for  Underwater 
Vehicles 

Acoustic  communication  has  been  the  subject  of  research  for  over  three  decades.  Due 
to  the  advent  of  low-power  Digital  Signal  Processors  (IDSPk)  acoustic  modems  suitable 
for  deployment  in  lAUVk  have  been  developed  and  are  now  commercially  available. 

The  acoustic  modem  which  we  will  refer  to  in  this  thesis,  the  micromodem ,  has 
been  developed  by  the  Woods  Hole  Oceanographic  Institution.  Figure  !2-6al  shows 
the  base  board  (top)  and  the  additional  IDSPI  The  transducer  for  transmitting  and 
receiving  is  either  directly  mounted  to  the  vehicle’s  hull  IfigurelH^Tal)  or  attached  to  a 
towfish  (figure  l2-6bl) .  The  transducer  consists  of  a  piece  of  ceramic  which  in  transmit 
mode  expands  and  contracts  and  creates  a  transversal  pressure  wave  which  travels 
through  the  water  body.  The  incoming  pressure  wave  excites  the  transducer  of  the 
receiving  modem  which  creates  an  electric  signal  that  is  interpreted  by  the  modem. 

Synchronized  Data  Transmission 

An  important  feature  provided  by  thclWHOTI  micromodem  is  its  ability  to  synchronize 
data  transmission  to  an  external  signal.  A  timing  board  with  a  very  precise  oscillator 
can  provide  a  Pulse  Per  Second  (IPPSI)  signal.  This  oscillator  is  synchronized  to 
the  global  clock  signal  of  the  IGPSI  when  the  vehicle  is  on  the  surface.  During  data 
transmission  the  modem  ensures  that  the  start  of  the  message  is  synchronized  to 
the  IPPSI  signal.  A  receiving  modem  on  a  different  vehicle  which  also  has  access  to 
the  globally  synchronized  IPPSI  signal  can  determine  the  Time-of-Flight  (ITOFI)  by 
registering  the  time  at  which  the  first  symbol  of  the  incoming  message  was  received. 
The  IPPSI  feature  can  only  resolve  the  ITOFI  within  one  second,  but  by  embedding 
a  time  stamp  in  the  data  message  longer  ITOFI  can  be  resolved.  The  syncing  of 
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Figure  2-7:  Range  estimate  from  globally  synchronized  data  transmission.  On  the 
surface  all  vehicles  synchronize  their  clock  to  the  global  IGPSltime.  When  IAIJVI  1 
transmits  a  message  it  is  synchronized  to  the  full  second  (here  t=100  s).  I  AT  J  VI  2 
receives  the  message  0.210  s  later  and  uses  he  measured  speed  of  sound  to  calculate 
the  distance.  IAIJVI  3  receives  the  message  1.386  s  after  it  was  broadcast  and  uses  the 
time  stamp  in  the  message  to  de-alias. 


Figure  2-8:  Multi-path  propagation:  in  addition  to  the  direct  path  (1)  there  are 
longer  paths  due  to  reflection  on  sea  bottom  (2),  sea  surface  (3)  and  a  temperature 
discontinuity  (4)  (thermocline) . 


the  vehicle’s  local  oscillator  and  the  time-synchronized  transmission  is  illustrated  in 
figure  12-71 


The  Acoustic  Communication  Channel 

The  speed  of  sound  in  water  is  around  1500  m/s,  which  is  significantly  lower  than 
the  transmission  speed  in  IRFI  or  cable  based  communication.  Additionally  the  speed 
depends  on  the  depth  as  well  as  the  water’s  temperature  and  salinity.  As  a  result, 
discontinuities  in  temperature  and  salinity,  as  well  as  the  sea  floor  and  the  air-water 
interface  reflect  the  pressure  waves.  For  a  given  physical  separation  of  the  transmitter 
and  receiver  there  often  will  exist  several  paths  with  different  lengths  along  which  the 
pressure  wave  travels.  Figure [231  shows  four  different  paths,  all  with  different  lengths. 
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Effects  of  the  Channel  on  the  Communication  Performance 

The  properties  of  the  acoustic  communication  channel  adversely  affect  data  transmis¬ 
sion  in  several  ways. 

Due  to  the  slow  transmission  speed,  the  different  path  lengths  in  figure  12-81  will 
cause  interference.  For  example  a  symbol  so  transmitted  at  time  to  traveling  along 
path  1  in  figure  [2^8]  will  be  received  at  time  U-  Then  a  symbol  Si  transmitted  at  time 
t\  also  traveling  along  path  1  will  collide  at  the  receiver  at  time  t:i  with  a  “delayed 
copy”  of  symbol  s0  which  traveled  along  path  2. 

The  modem’s  carrier  frequency  is  directly  related  to  the  size  of  the  transducer. Thus 
frequencies  below  ~5  kHz  are  infeasible  as  the  required  transducer  would  be  too  large 
for  most  lAIJVk  As  the  attenuation  of  sound  underwater  increases  linearly  with  fre¬ 
quency  the  upper  bound  of  the  spectrum  which  still  provides  useful  communication 
ranges  is  approximately  40  kHz.  To  avoid  interference  as  described  above,  acous¬ 
tic  modems  often  cycle  through  frequency  slots  transmitting  consecutive  symbols  in 
different  frequency  bands.  This  makes  modem  communication  more  robust  by  avoid¬ 
ing  interference,  but  further  limits  the  usable  bandwidth.  Due  to  these  bandwidth 
constraints,  data  rates  for  acoustic  communication  will  be  limited  to  0(  10  kbps). 

Another  source  of  interference  is  the  presence  of  background  noise.  Surface  waves 
and  marine  mammals  as  well  as  the  noise  caused  by  the  vehicle’s  propulsion  and 
navigation  system  often  occupy  the  same  part  of  the  spectrum  as  the  one  used  by  the 
acoustic  modem. 
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Chapter  3 

Cooperative  Localization 


3.1  Probabilistic  State  Estimation 

The  goal  of  any  localization  technique  is  to  maintain  an  estimate  of  the  vehicle’s 
current  pose  using  information  obtained  from  vehicle  sensors  and  control  commands 
issued  to  the  actuators  of  the  vehicle.  As  the  readings  from  real  sensors  are  noisy  and 
the  effect  of  a  control  command  cannot  be  predicted  perfectly,  the  noise  needs  to  be 
modeled  accordingly  and  the  resulting  uncertainty  in  the  pose  estimate  needs  to  be 
represented. 

3.1.1  State  Representation 

In  the  most  generic  case  of  a  vehicle  operating  in  3D-space,  such  as  I AI J  VI;  andlUAVk 
the  state  is  treated  as  a  vector  of  random  variables  comprised  of  the  vehicle’s  pose 
which  is  its  position  in  a  (global)  reference  frame  [x,y,z]T  and  the  orientation.  For 
the  rest  of  the  discussion  we  assume  that  the  orientation  is  given  in  Euler  angles 
(0,  tp,  9).  The  pose  vector  at  time  t  is  then  x(t )  =  [x,  y,  z ,  0,  ip,  0]T .  Beside  the  pose, 
the  state  vector  can  also  contain  the  first  and  second  time  derivatives  of  the  pose 
vector.  Appendix  E]  illustrates  the  pose  vector  of  a  typical  I  ATI  VI 

3.1.2  Motion  Model 

To  evaluate  the  effects  of  control  inputs  uji]  on  the  pose  vector,  we  need  a  motion 
model  that  can  predict  a  future  pose  xt2  based  on  the  current  pose  xtl  and  the  current 
control  inputs  utl .  The  continuous-time  model  for  the  vehicle  state’s  speed  and  rate 
components  is 


xt  =  f(xt,ut)  (3.1) 

The  function  /(•)  in  equation  13.11  is  usually  non-linear  and  can  be  very  complex. 
It  depends  on  the  vehicle’s 

1For  the  remainder  of  section  IXTl  we  denote  the  time  dependency  by  a  subscript,  i.e.  xt=x(t) 
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T  , 0f 
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Figure  3-1:  Simple  lAlJVI  motion  model 


•  shape 

•  size 

•  weight 

•  actuators 

•  operating  environment  (air,  water,  vacuum,  . . . ) 

The  more  complex  the  model,  the  more  accurately  it  can  represent  the  vehicle 
dynamics  and  provide  a  better  prediction  of  the  future  pose,  but  obtaining  such  a 
model  requires  detailed  knowledge  of  the  structure  as  well  as  the  parameters  listed 
above.  A  large  body  of  literature  exists  specifically  to  provide  models  for  lAUVk 

mmmmm- 

Figure  13-11  shows  a  simple  kinematic  2D-model  for  an  IAIJVI  in  which  the  control 
input  is  given  by  the  commanded  forward  speed  xt  =  ut  and  rudder  setting  leading 
to  a  turning  rate  6t. 

Equation  fl3.ll)  represents  an  ideal  motion  model.  As  even  the  most  complex 
model,  however,  cannot  fully  represent  a  real  robot  operating  in  a  real  environment, 
uncertainties  will  remain.  In  an  underwater  scenario,  these  could  be  currents  which 
cannot  be  observed.  Therefore  we  need  to  include  a  noise  term  wt  that  accounts  for 
these  uncertainties.  Equation  (I3.1|)  then  becomes 


xt  =  f(xt,Ut,wt)  (3.2) 

As  the  state  estimation  is  normally  carried  out  on  a  digital  computer,  we  need 
to  operate  in  discrete  time  space.  We  choose  a  sampling  period  AT  small  enough 
such  that  x  and  u  and  can  assumed  to  be  constant  within  that  interval.  In  a  real 
system,  the  frequency  fu  =  is  usually  the  same  as  the  update  frequency  of  the 
control  loop.  High-frequency  components  not  captured  by  fu  are  modeled  as  noise 
and  contribute  to  w.  The  sampling  period  is  constant  and  known  and  we  represent 
the  time  tk  simply  by  the  index  k  with  tk  =  k  ■  AT.  The  discrete  version  of  (13.21)  is 
then 
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(3.3) 


3.1.3  Measurement  Model 

Each  robot  usually  carries  various  sensors  that  provide  information  about  its  own 
state  (proprioceptive)  as  well  as  about  the  environment  (exteroceptive).  Many  of  the 
sensors  used  on  I  ATI  Vf  are  described  in  chapter  12.1.11  Measurements  provided  by  these 
sensors  are  denoted  zlk,  where  i  is  the  sensor  number.  We  combine  the  measurements 
from  all  sensors  at  time  k  into  the  measurement  vector  zk.  While  the  proprioceptive 
measurements  only  depend  on  the  state  of  the  robot  xk,  the  exteroceptive  measure¬ 
ments  also  depend  on  the  state  of  the  observed  feature.  We  use  m\,  as  a  description 
of  the  feature.  A  detailed  explanation  of  the  structure  of  mlk  will  be  presented  in 
section  13.2.21  As  a  result  the,  usually  non-linear,  function  g  representing  all  sensors 
is  dependent  on  the  state  of  the  robot  xk  and  all  observed  features  mk. 


zk  =  g{xk,mk )  (3.4) 

As  every  real  sensor  measurement  is  subject  to  error  (with  the  characteristics  often 
provided  by  the  sensor’s  manufacturer),  the  error  needs  to  modeled.  This  requires  an 
additional  sensor  noise  term  vk  such  that  the  true  sensor  model  becomes 


zk  =  g(xk,mk,vk) 


(3.5) 


3.1.4  Probabilistic  Representation 

In  order  to  solve  the  problem  of  localization  in  the  presence  of  model  errors  as  well  as 
noisy  control  and  measurement  data,  we  choose  a  probabilistic  formulation.  The  pose 
xt  as  well  as  the  control  inputs  ut  and  the  measurement  zt  are  modeled  as  random 
variables.  The  evolution  of  the  pose  xt  is  now  modeled  in  two  steps.  First,  the  state 
transition  probability  and  second,  the  measurement  probability. 


State  Transition  Probability 

The  state  transition  probability  models  the  effect  of  the  control  inputs  ut  on  the 
robot  state  xt.  As  the  state  xt  is  stochastically  generated  from  the  state  xt_i  its 
distribution  can  be  expressed  by  the  conditional  probability  p(xt\xo:t-i,  Zi:t-i,  Ui:t) . 
By  properly  defining  the  state,  we  can  enforce  conditional  independence  [84]  such 
that 


p(xt\xo:t—i,  z i:t— i,  U\:t)  p{xt |«Ef— i,  u%) 


(3.6) 


If  (j3.fi j)  applies,  xt  is  complete  [HI]. 
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Measurement  Probability 

Similarly,  the  the  stochastic  measurement  model  can  be  expressed  as  a  conditional 
probability  p(zt\x0:t,  Ui:t)  and  if  xt  is  complete  we  have 


p(zt\xQ:t,  zi:t-i)  =p(zt \xt) 


(3.7) 


3.1.5  Bayes  Filter 

A  general  algorithm  to  track  the  new  state  xt  based  on  a  posterior  distribution  cct_i,  a 
control  input  ut  and  observations  (measurements)  zt  is  the  Bayes  filter.  It  implements 
the  state  evolution 


p(*t|itt_i,  zt_ i)  p(xt+1\ut,  zt) 

as  a  two  step  process,  the  predict  and  update  step  shown  in  algorithm  El 


1:  for  all  t  do 

2:  p(xt\ut,zt_l,xt_1)  =  J  p(xt\ut-i,xt_1)p(xt_1)dxt_1 

3:  p(xt\ut,  zt,  xt_ i)  =  T]p(zt\xt)p(xt\ut,  Zt) 

4:  end  for 

Algorithm  2:  The  Bayes  filter 

In  the  predict  step  (line  El)  the  filter  processes  the  control  input  ut.  The  probability 
distribution  over  the  state  xt-\  is  updated  by  integrating  (summing  in  the  discrete 
case)  over  the  state  xt-\  times  the  probability  that  the  control  input  ut  applied  to 
the  motion  model  causes  a  transition  to  xt. 

The  update  step  (line  [3J)  multiplies  the  hypothesis  of  the  prior  state  distribution 
xt  by  the  probability  that  the  measurement  zt  may  have  been  observed.  As  the 
integral  over  this  updated  posterior  state  distribution  may  not  integrate  to  1,  the 
normalization  factor  rj  is  applied  [Mj. 

Note  that  there  does  not  need  to  be  an  update  step  for  every  prediction  step.  If 
no  measurements  are  available,  the  update  step  will  be  omitted. 

The  Bayes  algorithm  iterates  recursively  over  the  predict  and  update  step.  It  needs 
an  initial  distribution  x0.  If  the  initial  position  is  perfectly  known,  the  distribution 
over  Xq  is  a  single  realization  of  the  state  vector  with  a  probability  of  1  or  a  uniform 
distribution  over  the  state  space  if  it  is  perfectly  unknown  but  finite. 


3.2  Cooperative  Navigation  in  the  Context  ofISLAM 


The  problem  of  localization  can  be  viewed  in  the  more  general  context  of  Simultaneous 
Localization  and  Mapping  (ISLAMI) .  As  a  large  body  of  literature  addresses  thelSLAMl 
problem,  by  phrasing  ICNI  in  the  context  of  ISLAM!  we  can  leverage  the  results  of 
research  in  that  area  to  develop  effective  localization  methods.  We  will  describe  the 
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(a)  Human-readable  Map  (b)  Grid-based  Map 

Figure  3-2:  Three  representations  of  the  same  environment.  A  human  readable  street 
map  f!3-2al)  and  its  grid-based  (13-21)11  and  feature-based  (l3-2cl)  parametrization. 


(c)  Feature-based  Map 


general  formulation  of  the  ISLAMI  problem  and  a  particular  representation  which  is 
equivalent  to  the  problem  ofICNl  A  thorough  description  of  the  various  aspects  of  the 
ISLAMI  problem  is  beyond  the  scope  of  this  thesis,  but  a  detailed  introduction  as  well 
as  various  implementations  of  a  solution  are  presented  by  Thrun  [84],  Walter  [90], 
Olson  [E5],  Eustice  [31 J  and  Frese  [3EJ. 


3.2.1  General  Formulation  of  the  ISLAMI  Problem 


The  objective  of  ISLAMl  is  to  enable  a  mobile  robot  to  use  its  sensors  to  build  a  map 
of  its  environment,  while  at  the  same  time  localizing  itself  within  this  map. 

As  the  robot  uses  noisy  sensor  readings  to  build  a  local  map,  the  map  M  itself 
has  to  be  expressed  in  a  probabilistic  framework.  ISLAMI  addresses  this  problem  by 
adding  the  map  to  the  estimated  posterior. 


p(xt+i,M\ut,zt) 


(3.8) 


3.2.2  Map  Representation 

To  represent  the  environment  sensed  by  the  robot,  various  parametrizations  are  possi¬ 
ble.  The  two  most  common  representations  are  grid-based  and  feature-based.  Figure 
illustrates  how  the  same  human-readable  map  (fig.  1 3- 2 all  can  have  the  two  represen¬ 
tations  described  below  thg.  !3-2bl  and  fig.l3-2cl). 
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Grid-based  representation 

In  the  most  common  type  of  grid-based  representation,  the  environment  is  discretized 
into  a  finite  number  of  cells.  The  map  M  then  becomes  a  set  of  n  grid  cells,  where  each 
element  is  a  binary  occupancy  value  indicating  if  this  cell  is  occupied  (blocked)  or 
free  (traversable)  and  p{rnf)  indicates  the  probability  of  the  cell  m,  being  occupied. 
Grid  maps  assume  the  probabilities  p^mf)  to  be  independent  [83J. 


M  =  {mo,  .,mn} 


j  1  “occupied” 
\  0  “free” 


Figure  l3~-2bl  shows  a  grid-based  map  representation.  The  gray-scale  value  of  each 
cell  encodes  pirnf). 


Feature-based  representation 

Feature-based  representations  parameterize  the  environment  into  a  set  of  landmarks, 
where  the  type  of  extracted  landmarks  is  usually  constrained  by  the  sensor’s  capa¬ 
bilities.  The  map  M  =  {mo, . . . ,  m*, . . . ,  mn}  is  then  a  set  of  vectors  m*  as  each 
feature  can  have  several  parameters  describing  it  such  as  position,  orientation  and 
color.  Each  element  m\  of  m,  =  [m], . . . ,  mj, . . . ,  m”]  is  a  random  variable  for 
parameter  j. 

A  special  case  of  a  feature  is  a  point  feature  which  is  assumed  to  be  infinitely  small 
in  size  and  is  represented  by  its  position  and  a  unique  identifier.  The  feature  vector 
rrii  for  a  point  feature  consists  of  a  distribution  over  its  position  and  the  unique  id 
with  p{m\  =  id)  =  1. 

In  figure  !3-2cl  two  kind  of  features  were  extracted  from  by  the  robot,  the  roads 
ri  =  {ri,  •  •  •  ,  r5}  and  the  intersections  U  =  {ii,^}.  The  intersections  are  represented 
as  point  features. 

While  ICNI  does  not  explicitly  track  a  feature  map  M  like  ISLAMI  does,  the  fol¬ 
lowing  sections  will  illustrate  how  the  concept  of  localization  in  a  feature-based  map 
is  an  adequate  description  of  the  ICNI  problem.  Also,  the  problems  encountered  in 
feature-based  maps,  such  as  the  possible  correlations  of  their  pose  ra,  have  their 
correspondence  in  the  ICNI  problem  and  need  to  be  addressed. 


3.3  Probabilistic  Representation  of  ICNI 

After  introducing  the  concept  of  probabilistic  state  estimation,  its  general  solution, 
the  Bayes  filter,  and  map  representations,  we  now  phrase ICNl as  a  localization  problem 
in  a  feature-based  map.  The  nomenclature  introduced  in  this  section  will  be  used 
throughout  the  following  discussion. 

Figure  13-31  shows  a  set  of  5  vehicles  Vi  =  {1, ...  ,5}  in  the  ICNI  setting.  All 
vehicles  maintain  a  distribution  of  the  estimate  of  their  state  cc  jE  During  a  broadcast, 

2 For  the  remainder  of  the  thesis  the  subscript  will  denote  a  vehicle,  i.e.  x%  represents  the  state 
of  vehicle  3. 
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Figure  3-3:  A  set  of  5  vehicles  in  a  ICNI  setting,  with  each  vehicle  i  maintaining  a 
distribution  over  its  state  Xi  and  obtaining  measurements  from  other  vehicles  which 
consist  of  the  state  estimate,  the  unique  id  of  the  other  vehicle  and  the  range  between 
the  two  vehicles. 


a  vehicle  (iq  in  fie.  13-311  sends  a  parametrization  of  this  distribution  along  with  its 
unique  id.  The  receiving  vehicles  (only  v\  in  fig.  13-AD  receive  this  broadcast  and 
also  obtain  an  intra- vehicle  range.  Each  receiving  vehicle  treats  the  reception  as  the 
exteroceptive  measurement  of  a  known  feature  represented  by  the  feature  vector  m. 
At  each  reception  the  probability  distribution  over  the  transmitting  vehicle’s  pose 
(here  X2)  is  contained  in  the  data  packet  and  the  range  to  the  transmitting  vehicle 
Z\  2  =  r  1,2  is  obtained  from  onboard  sensors  (e.g.  modem).  As  a  result,  no  vehicle 
needs  to  add  a  feature  map  M  to  its  state  vector,  unlike  vehicles  performing  ISLAMl 
where  M  is  part  of  the  posterior  (13. 81).  Just  as  in  ISLAMl  however,  measurements  can 
become  dependent  and  we  may  need  to  store  and  transmit  additional  information  in 
the  feature  vector  in  order  to  integrate  the  measurements  such  that  these  correlations 
are  accounted  for.  Section  lT6l  addresses  the  problem  of  dependent  measurements. 


3.4  Localization  Algorithms 


In  this  section,  we  will  introduce  the  two  most  popular  implementations  of  the  Bayes 
filter,  the  Kalman  and  the  Particle  filter,  which  are  widely  used  for  localization  and 
in  the  ISLAMl  context.  The  main  difference  between  the  filters  is  how  the  distribution 
of  the  state  is  represented.  The  different  representations  of  the  distribution  also  lead 
to  different  implementations  of  the  predict  and  update  step. 

We  will  also  examine  the  computational  requirements  and  the  amount  of  infor¬ 
mation  that  needs  to  be  transmitted  in  the  ICNI  scenario  outlined  in  13.31  The  latter  is 
particularly  important  as  the  low  bandwidth  of  the  acoustic  communication  channel 
severely  constrains  the  amount  of  data  that  can  be  transmitted  in  the  m. 
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Figure  3-4:  A  set  of  5  vehicles  performing ICNI  using  anIEKFl  Each  vehicle  i  maintains 
the  distribution  over  its  state  (red)  through  a  mean  vector  nt  and  the  associated 
covariance  matrix  Pj.  This  information,  along  with  the  unique  id  is  broadcasted  to 
other  vehicles. 


3.4.1  Extended  Kalman  Filter 

The  original  Kalman  Filter  (IKFD  [5T1]  requires  the  function  /(•)  (13.31)  and  g(-)  (13.51) 
to  be  linear,  but  as  even  the  simple  motion  model  shown  in  figure  13-11  is  non-linear 
we  use  the  lEKFl  which  linearizes  equations  /(•)  and  g(-)  [81]. 

The  lEKFl  [47J  filter  is  a  member  of  the  family  of  Gaussian  Liters  and  the  proba¬ 
bility  density  over  the  state  x  is  assumed  to  be  normally  distributed.  The  Gaussian 
distribution 


V (*)  =  =  exp  ffP  *(*  -  fi) 

can  be  fully  described  by  its  mean  and  covariance.  The  state  x  is  characterized  by 
a  mean  vector  fi  e  Mn,  with  the  same  dimensionality  n  as  the  vector  x,  and  the 
covariance  matrix  P  e  Mn  x  Mn  which  is  positive  and  semidefinite  [84j.  As  a  result 
only  uni-modal  distribution  centered  around  /J,  can  be  modeled. 

When  applying  the  lEKFl  to  solve  the  problem  of  ICNI  we  assume  that  all  n  vehicles 
of  the  set  of  participating  vehicles  V’j  =  {l,...,i,...,n}  maintain  a  vector  which  con¬ 
sists  of  the  mean  vector  x^k)  =  [xi(k),yi(k),  Zi(k)]T  =  fi^k)  =  [fixi(k),  fiyi{k),  fizi{k)]T 
that  contains  the  an  estimate  of  their  position  at  time  /c,  as  well  as  Pi 


Pi(k) 


@xx  (^) 

dyx  if) 
azx2(k) 


@xy  if) 

Gyy  ( k ) 

Vzy2(k) 


<?xz2{k) 

Vyz2{k) 

Vzz2(k) 


the  covariance  matrix  describing  the  uncertainty  associated  with  that  estimate. 


In  the  following  description  the  line  numbers  refer  to  algorithm  [3l 
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Prediction 

Whenever  vehicle  i  =  1  obtains  proprioceptive  measurements  Ui(k)  from  its  dead¬ 
reckoning  sensors,  /j,1  (k)  and  P\{k)  are  propagatecd 


Mi  (k  +  1) 

=  0(tti(*;),Mi(fc)) 

(3.9) 

Pi(k  +  1) 

=  G^k  +  ^P^G^k  +  l) 

1  Qi{k  +  1) 

(3.10) 

where  Q1(/c  +  1)  is  a  matrix  where  the  elements  contain  the  variances  of  the  motion 
model  (w  in  eq.  (13.1  ID  which  is  modeled  as  zero- free  Gaussian  noise  and  G±(k  +  1)  is 
the  Jacobian  containing  the  partial  derivatives  of  g  (line  131  and  H]) . 


dgjuijk  +  1),  xi(k)) 
dxi(k) 


Xl=/JL1(k+l) 


Update 

If  vehicle  1  receives  a  broadcast  from  vehicle  2  at  k  that  contains  J i2(l)  and  P2{1) 
together  with  an  intra- vehicle  range  measurement  rij2(fc)  (line  Oj,  it  uses  this  infor¬ 
mation  to  update  its  estimate  of  its  own  position  as  follows: 

First,  it  computes  what  the  predicted  range  z\>2 (k)  between  the  two  vehicles  would 
be,  based  on  their  estimated  position. 


zi,2(k)  =  || ^(fc)  -  n2(k) ||2  (3.11) 

The  difference  between  the  predicted  measurement  and  the  measured  distance  Zij2(k)  — 
7*1,2  (k)  represents  the  innovation  (lineEJ). 

The  covariance  matrix  of  vehicle  1  and  vehicle  2  are  combined  (line  |8j)  into 


i\2(fc  +  l) 


P\{k  +  1)  _  0 

0  P2(fc  +  1) 


(3.12) 


Note  that  P\(k  +  1)  and  P2{k  +  1)  are  assumed  to  be  deindependent  {P\,2{k  +  1) 
is  diagonal).  This  is  not  generally  true  and  if  the  non-zero  off-diagonal  elements  of 
Pi,2(k  +  1)  are  ignored,  the lEKFl can  become  overconfident  and  diverge.  As  keeping 
track  of  these  elements  in  IGNI  is  very  difficult,  however,  we  propose  a  method  in 
section ITfil which  keeps  P\{k  +  1)  and  P2{k  +  1)  independent. 

We  compute  the  Jacobian  Hi^ik  +  1)  that  contains  the  derivatives  of  the  range 
measurement  with  respect  to  the  position  of  vehicle  1  and  2  (time  index  k  omitted 
on  matrix  components)  (line  EO) . 


Hh2(k  +  1) 


dr  dr  dr  dr  dr  dr 

dJZ^1  djMJ1  dzi  djix2  dJhj  2  ^2 


3  n  and  P  denote  the  state  after  the  predict  step,  but  before  the  update  step. 
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Using  the  residual  covariance  (line  ITOl)  and  the  variance 

Slf2(k  +  1)  =  Hh2(k  +  l)P1(2(fc  +  1  )Hl2{k  +  1)  +  a2r 

and  ar  associated  with  the  exteroceptive  (range)  sensor  we  compute  the  Kalman  gain 
(line  ED 


Klt2(k  +  1)  =  Pi,2(fc  +  l)Hl2{k)S^{k  +  1) 

that  represents  a  weighting  factor  for  how  much  the  measurement  will  affect  the 
updated  position.  Using  the  innovation  z\>2{k)  —  n )2(fc)  and  the  Kalman  gain,  the 
updated  position  estimate  is 

Mi  (k  +  1)  =  Mi  (k  +  1)  +  K\2(k  +  1)  ^1,2  (k)  —  ri)2  (k)j  (3.13) 

(line  ED  arid  the  combined  covariance  is 


Pi,2(fc  +  1) 


'  Pi(k  + 1)  P12(k  +  1)' 

_P21(k  +  l)  P2(k  + 1)  _ 

(l6x6  -  K1<2(k  +  l)Jfi,2(fc))pli2(fc  +  1)  (3.14) 


from  which  we  can  extract  the  updated  covariance  estimate  for  vehicle  1  P±(k  +  1). 
Note  that  we  also  obtain  an  updated  estimate  for  the  position  and  covariance  of 
vehicle  2  P2(k  +  1)  and  n2(k  +  1). 


Characteristics 

Under  the  assumption  that  the  initial  state  error  as  well  as  all  noises  entering  into 
the  system  have  a  Gaussian  distribution  and  that  the  underlying  model  is  linear, 
the  IKFI  is  the  optimal  estimator  in  the  sense  that  it  minimizes  the  Minimum  Mean 
Squared  Error  iihmshi  ma.  As  a  result  and  because  of  its  simplicity,  tractability 
and  robustness,  the  IEKFI  is  the  most  common  algorithm  used  today  for  tracking 
and  estimation.  For  the  particular  case  of  underwater  ICNl  several  key  assumptions 
necessary  to  guarantee  optimality  in  the  IMMSElsense  cannot  be  made  and  can  lead 
to  very  large  position  estimation  errors. 

Non-linear  Motion  and  Measurement  Model  Even  the  most  basic  motion  model, 
represented  by  /  in  flUD  which  maps  body  velocities  into  a  global  coordi¬ 
nate  frame  has  non-linear  components  (see  fig.  EED-  Similarly  the  measurement 
model  g  (13.51)  used  in  the  update  step  (13.111)  is  non-linear.  These  non-linearities 
can  lead  the  IEKFI  to  be  unstable  [48j . 

Non-Gaussian  Noise  The  sensors  used  for  underwater  navigation  often  have  very 
non-Gaussian  noise  characteristics.  Particularly  the  various  components  con¬ 
tributing  to  the  error  in  a  heading  measurement  derived  from  a  magnetic  com- 
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Require:  /i1(0),Pi(0) 


Ati(0),Pi(0) 

loop  {do  this  at  each  time  step  k } 

Mi  (k  +  1)  =  giu^iH^k)) 

P  \{k  +  1)  =  G\{k  +  l)Pi  ( k)G t  (k  +  1)  +  Q\(k  +  1) 
if  measurement  m2(k)  is  available  then 
M20) 


9: 

10: 

11: 

12: 

13: 


m2(fc)  = 


P2(k) 

id  =  2 


<A,2(&)  = 

P1(2(fc+1)  = 


-_ri’2 
MiW -M2WII2 
Pi(fc  +  1) 
0 


P2(*:  +  l) 

tt  /7  \  _  <9r  Or  Or  Or  Or 

0^!  0yx  O^i  0/I2  dy2  l 

Sh2(k  +  1)  =  Hi,2(fc)Pi)2(fc  +  1  )Hl2(k)  +  ar 
Kh2(k  +  1)  =  Pli2(fc  +  l)H?;2(A:)5^(fc  +  1) 

Mi (k  I  1 )  =  //( (/>'  I  1 )  :  Ki^2(k  +  1) (ci.2(/,‘)  —  /'i.2( 

Pi(k  +  1)  P\2{k  +  1) 

P21(fc  +  1)  P2(fc  +  1)  . 

/6x6  -  Kh2(k  +  l)Hlt2{k))p1)2{k  +  1) 


P1)2(fc  +  1)  = 


14:  end  if 

15:  end  loop 


Algorithm  3:  IEKFI  algorithm  for  a  vehicle  1  initialized  with  a  pose  /x1( 0)  and  associ¬ 
ated  uncertainty  Pi(0),  moving  in  2D  by  executing  control  commands  and  occasion¬ 
ally  receiving  range/position  pairs  m2(k)  from  another  vehicle  2. 


pass  (see  section  12. 1.11)  are  hard  to  fit  to  a  Gaussian  noise  model.  For  exterocep¬ 
tive  measurements  Olson  et  al.  show  that  for  ILBLI  beacons  (see  chapter  I2.1.1D 
Gaussian  noise  is  a  very  poor  approximation  due  to  the  large  number  of  out¬ 
liers  |(HJ. 

Uni- modal  probability  distribution  As  the  probability  distribution  for  the  lEKFl 
is  only  modeled  by  the  mean  and  covariance  of  a  single  Gaussian,  the  Kalman 
filter  can  only  track  a  single  hypothesis  of  the  state  vector.  If  the  initial  state 
/x(0)  is  not  known  and  needs  to  be  represented  by  a  uniform  distribution  over  a 
finite  state  space  or  the  initial  information  about  the  state  contains  ambiguity, 
the  resulting  distributions  cannot  be  tracked  by  an  IEKFI 

Computation  and  Bandwidth  Requirements 

The  predict  step  of  the  IEKFI  is  computationally  efficient  for  very  large  state  vectors, 

but  the  computation  and  memory  requirement  for  the  update  step  grows  with  0[n 2) 
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where  n  is  the  size  of  the  state  vector.  Fortunately  the  state  vector  for  underwater  ICNI 
is  small  and  updates  are  very  infrequent  (9(0.1  Hz).  As  a  result,  the  computational 
requirements  on  a  vehicle  performing  lEKFlbascd  ICNI  are  very  small.  Additionally 
the  compact  formulation  of  the  state  distribution  leads  to  a  message  size  of  only  a 
few  bytes,  which  is  well  suited  for  the  low-bandwidth  channel. 


3.4.2  Particle  Filter 

The  Particle  Filter  [2_6]  is  a  member  of  the  family  known  as  Monte  Carlo  methods  and 
fundamentally  differs  from  the  IEKFI  in  the  way  the  probability  distribution  over  the 
state  is  represented.  The  distribution  is  represented  by  a  fixed  number  (n)  of  samples 
from  the  distribution.  Each  sample  Cj  of  set  C  =  {ci, ...  ,Ci, ,  cn }  consists  of  an 
instantiation  of  the  state  vector  ay  and  an  associated  weight  wy. 

ci  =  [xi,wi]  i  =  [l,...,n] 

The  probability  distribution  is  recursively  updated  by  a  two  step  process.  First  the 
motion  model  is  applied,  then  the  measurement  update.  At  each  time  step  every 
particle  is  affected. 

Several  variations  of  the  algorithm  exist  and  we  show  one  possible  implementation 
particularly  addressing  the  ICNI  scenario  outlined  in  13.31  Here  each  vehicle  maintains 
its  own  particle  filter  only  to  track  its  own  state. 

The  following  section  explains  the  individual  steps  and  the  line  indications  refer 
to  algorithm  |U 


Initialization 

The  set  C (0)  which  we  use  to  initialize  our  IPFI  is  generated  by  drawing  n  samples 
Xi  with  i  =  [1 , ...  ,n\  from  the  distribution  representing  the  initial  assumption  about 
our  state  X  (line  [2])  •  The  distribution  function  can  be  completely  arbitrary.  We  can 
sample  from  a  uniform  distribution  over  the  entire  state  space,  as  long  as  it  is  finite, 
in  case  we  are  ignorant  of  our  initial  state,  or  all  n  samples  are  the  same  instantiation 
of  the  state  vector  in  case  our  initial  state  is  perfectly  known.  Independent  of  the 
distribution  function  from  which  we  are  sampling  from,  we  assign  an  equal  weight  to 
all  particles  «y  =  ^  Vi  (line El). 


Prediction 

For  a  single  prediction  step,  the  motion  model  is  applied  to  each  particle  ry  individu¬ 
ally. 

In  order  to  update  a  single  particle  we  draw  a  sample  from  the  individual  distri¬ 
bution  of  each  variable  from  the  control  space  U  (line  [8|)  and  apply  the  motion  model 
using  these  samples  (line  [9]) .  Note  that  the  distribution  over  state  variables  can  be 
different  for  each  component  of  the  state  vector  and  arbitrary. 
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Update 

If  a  measurement  m3(k)  =  [C3  (k),  id  =  j,  r^)] ,  as  shown  in  figure  13-51  is  available  we 
evaluate  the  function  e(xi(k),m3 (k))  for  each  particle.  The  function  e(.)  computes 
a  likelihood  of  each  particle  x,(k)  belonging  to  the  distribution  represented  by  C3 
transformed  along  r3 . 

A  possible  implementation  of  e(xi,m3  (k))  is  to  compute  the  weighted  average 
fj,J(k)  of  all  particles  c[  from  the  distribution  C3(k) 

n 

VJ(k)  =  J2xi(k)wl(k)  (3.15) 

2=1 

and  assume  a  normal  distribution  of  the  range  error  with  r  =  A/"(0,  ar).  The  likelihood 
then  becomes 


Wi(k)  =  e(xi(k),  ii(k)3)  =  p(xi(k ))  =  1  exp  f- — till — rlL\  y  Xi(k) 

a  J  l 7 t  ar  \  2,  ar  J 

Note  that  only  the  weights  «',(/,:)  are  updated,  not  the  particle  positions.  As  the 
sum  of  all  weights  in  the  set  does  not  necessarily  add  up  to  1  the  particle  weights  are 
renormalized  to  enforce  'YH=iw *  =  1  (line[HJ). 


Resample 


Each  update  reevaluates  for  all  particles  how  likely  the  hypothesis  is  that  the  vehicle’s 
state  is  Xi  as  represented  by  the  particle  c,.  This  likelihood  is  represented  by  the 
weight  Wi .  After  a  series  of  updates,  the  distribution  of  weights  across  the  particles 
can  become  very  uneven  which  means  that  the  set  C%  has  very  strong  hypotheses 
(high  weight)  and  many  very  weak  hypotheses  (low  weight).  The  goal  of  the  particle 
filter,  to  track  the  distribution  of  the  state  through  a  set  of  instantiations,  is  best 
achieved  if  all  samples  have  very  similar  weights. 

To  maintain  an  even  distribution  of  the  particle  weights  we  determine  the  number 
of  “effective  particles”  ALff  =  [0, 1]  with 


iVeff  = 


1 


n 


En  9 

i=i  wi 

which  is  a  measure  of  how  well  the  weights  are  distributed.  If  N,.^  drops  below  a 
threshold  7,  we  resample  by  building  a  new  set  C(k )  by  drawing  n  samples  from 
C[k  —  1)  where  each  particle  <7  ( k  —  1)  is  drawn  with  a  likelihood  w,  ( k  —  1)  (line  1201) . 
The  new  set  C(k)  now  represents  single  strong  particles  from  C ( k  —  1 )  by  a  number 
of  equally  weighted  particles  in  C(k)  while  very  weak  particles  might  not  be  drawn 
at  all,  they  “die”  (line  fT7T) .  This  is  a  problem  particular  to  the  IPFI 
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1: 

2: 

3: 

4: 

5: 

6: 

7: 


9: 

10: 

11: 

12: 

13: 

14: 

15: 

16: 

17: 

18: 

19: 

20: 

21: 

22: 

23: 


for  j  =  l:n  do  {initialize  all  particles} 
£,(0)  =  sample(A’x) 

#i(0)  =  yi(  0)  =  sampl  e{Xy) 

9i(  0)  =  sample^#) 


tCi(O)  =  ^ 

Ci(0)  =  [xi(0),Wi(0)] 


end  for 

loop  (do  this  at  each  time  step  k} 
for  i  =  l:n  do  {predict} 


Ui(k)  = 


Xi(k )  = 


^7  (^) 
0i(k) 
Xi(k ) 
Vi(k) 
9i{k) 


sample  (Uu) 
sample  (U$) 
Xi(k  —  1)  + 
Xi(k  —  1)  + 
0i(k) 


Ci(k )  =  [xi(k),Wi(k  -  1)] 


Ui(k) 
Ui(k ) 


cos(9i(k )) 
sin(9i(k )) 


end  for 

if  measurement  m(k)  is  available  then 
for  j  =  l:n  do  {update} 

Wi(k)  =  e(xi,m(k )) 


end  for 

for  i  =  1  :  n  do  {normalize  particle  weights} 

Wi(k)=wi(k)^-^m 

end  for 

if  Ne//  <  7  then  {resample  if  necessary} 
C(k )  =  resample  (C(/c  —  1)) 

end  if 
end  if 
end  loop 


Algorithm  4:  IPFI  algorithm  for  a  vehicle  initialized  with  a  pose  sampled  from  A, 
moving  in  2D  by  receiving  control  commands  U\  (/»:)  sampled  from  U  and  occasionally 
receiving  range/position  pairs. 


Characteristics 

By  representing  arbitrary  probability  distributions  over  the  state  and  to  incorporate 
non-linear  motion  and  measurement  models  without  linearization,  the  IPFI  avoids  two 
of  the  main  disadvantages  of  the  IEKF1  The  IPFI  only  maintains  a  finite  set  of  discrete 
hypotheses  about  the  vehicle’s  position,  unlike  the  Gaussian  distribution  in  the  IEKFI 
which  is  continuous  and  covers  the  entire  state  space.  To  avoid  “particle  depletion” , 
the  absence  of  particles  near  the  true  solution,  the  weighting  function  e(.)  (line  fill  in 
algorithm  0J)  must  be  carefully  chosen.  Other  techniques  add  random  samples  [84]  or 
add  additional  noise  to  the  samples  [34] .  It  is  important  to  note  that  while  the  IEKFI 
inherently  provides  a  “best  guess”  for  the  actual  position  (the  mean) ,  the  IPFI  does 
not.  A  control  algorithm  on  a  vehicle  however,  that  is  tasked  to  guide  the  vehicle 
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to  a  specific  location,  normally  requires  an  estimate  which  is  a  single  instantiation  of 
the  state  distribution.  A  simple  way  to  provide  a  single  position  estimate  which  can 
be  used  by  the  control  algorithm  of  the  vehicle,  is  to  compute  the  weighted  mean  of 
the  distribution  over  the  state  space  (13.151)  along  with  a  weighted  covariance  (13.161). 

n 

o-(k)  =  ^2(xi(k)  -  fi(k))2  Wi(k)  (3.16) 

2=1 

A  more  complex  method  first  discretizes  the  space,  where  the  likeliest  position  is 
assumed  to  be,  into  grid  points.  It  then  instantiates  a  Gaussian  distribution  on  every 
particle  and  sums  up  the  contributions  of  all  Gaussians  to  the  set  of  grid  points.  The 
grid  point  with  the  the  highest  combined  contribution  is  the  filter’s  estimate  of  the 
actual  position  [32]  • 

Computation  and  Bandwidth  Requirements 

The  computational  complexity  is  linear  in  the  number  of  particles.  The  computa¬ 
tion  required  to  carry  out  the  update  and  the  predict  step  for  each  particle  depends 
entirely  on  the  complexity  of  the  motion  and  measurement  model  respectively,  but 
with  the  scenario  given  in  typical  underwaterlCNI  application  IPFk  with  tens  of  thou¬ 
sands  of  particles  are  feasible.  Transmitting  the  entire  state  space  distribution  (C2  in 
figure  G22)  for  IGNI  lias  been  done  by  Fox  et  al.  [37],  but  given  the  low  bandwidth  avail¬ 
able  in  underwater  communication,  this  is  only  feasible  for  a  very  small  number  of 
particles.  Therefore,  in  order  to  apply  the  IPFIbased  IGNl  underwater  the  distribution 
needs  to  be  parameterized  before  being  transmitted  to  the  other  vehicle.  If  the  distri¬ 
bution  is  parametrized  by  computing  the  weighted  mean  (13.151)  and  covariance  (13.161) 
the  transmitted  data  is  the  same  as  in  the  IEKFI  case.  This  is  the  most  compact  way 
to  represent  the  distribution  which  makes  it  particularly  suited  for  low  bandwidth 
communication  at  the  expense  of  being  able  to  model  multi-modal  distributions.  One 
way  to  maintain  the  multi-modality  of  the  transmitted  distribution,  while  maintain¬ 
ing  a  small  set  of  information  which  needs  to  be  transmitted,  is  to  represent  it  as  a 
Gaussian  Mixture  Model  (1GMMD  as  proposed  by  Merwe  and  Wan  [EH], 


3.5  Multi-Robot  Localization 

As  lAUVk  have  only  recently  become  reliable  enough  to  allow  for  multi- vehicle  de¬ 
ployments,  only  a  small  number  of  experiments  involving  more  than  one  vehicle  have 
been  carried  out.  However  a  much  larger  body  of  literature  for  land  vehicles  and,  to 
a  lesser  degree  for  air  vehicles,  exists.  The  following  two  sections  provide  an  overview 
over  the  previous  work  carried  out  in  the  field  of  cooperative  localization  for  land  and 
air  (13.5.11)  and  underwater  vehicles  (13.5.21). 

We  then  present  our  ICNlalgorithm  in  section  13.5.31  It  is  particularly  designed  to 
work  with  the  navigation  information  available  on  lAUVk  and  the  infrequent  broad¬ 
casts  obtained  from  other  vehicles  through  the  underwater  communication  channel. 
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Figure  3-5:  A  set  of  5  vehicles  performing  ICNI  using  a  Particle  Filter.  Each  vehicle 
i  maintains  the  distribution  over  its  state  (red)  through  a  set  of  particles  Ct  = 
{cj, . . . ,  cf1}.  This  information,  along  with  the  unique  id  is  broadcasted  to  other 
vehicles. 


3.5.1  Land  and  Air  Vehicles 

Roumeliotis  et  al.  have  contributed  a  large  body  of  work  to  the  held  ofICNI using  indoor 
land  robots.  Early  work  relies  on  a  central  site  for  data  storage  and  processing  m 
With  this  setup,  the  authors  make  useful  insights  into  the  relationship  between  the 
number  of  cooperating  robots  and  the  individual  position  uncertainty.  The  result  is  an 
analytical  expression  for  an  upper  bound  on  the  growth  rate  of  the  overall  positioning 
uncertainty  for  the  group  m-  In  another  experiment,  the  central  filter  that  keeps 
track  of  the  state  and  covariance  of  all  vehicles  is  replaced  by  distributed  filters  that 
run  on  the  individual  members.  Agents  now  only  need  to  exchange  local  data,  but  as 
both  vehicles  are  required  to  transmit,  this  approach  does  not  scale  as  well  as  others 
that  rely  only  on  one-way  broadcasts  [73] .  Caglioti  et  al.  [fSJ  also  use  a  distributed 
filter  approach.  While  they  only  require  one-way  data  exchange  (broadcast),  these 
broadcasts  occur  very  frequently  and  their  method  relies  on  perfect  communication 
as  each  vehicle  is  required  to  receive  every  broadcast  for  the  successful  application  of 
their  method. 

Nettleton  uses  a  group  oflUAVk  to  build  a  map  of  observed  features  locally  on  each 
vehicle  while  relying  only  on  broadcast  traffic  without  the  requirement  that  each  ve¬ 
hicle  receives  all  transmissions  [H2j .  While  the ITIAVh  do  not  exchange  information  for 
navigation  the  cooperative  map  building  provides  insights  into  the  problem  of  fusing 
information  from  mobile  platforms  using  an  unreliable  communication  channel.  The 
problem  of  fusing  measurements  from  several  sources  while  properly  keeping  track  of 
common  information  has  been  addressed  by  Grime  [43].  Unlike  the  work  of  Roumeli¬ 
otis  et  al.  Grime  and  Nettleton’s  work  tracks  the  information  parametrization  of  the 
Gaussian  rather  than  the  standard  form.  In  the  information  form,  the  update  step 
is  simply  an  addition,  and  joint  information,  which  models  co-dependencies  among 
different  states,  can  be  subtracted  if  the  communication  topology  is  known.  As  the 
update  step  is  computationally  inexpensive  the  information  form  is  well  suited  to 
integrate  large  amounts  of  information  from  cooperating  vehicles  ra. 
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The  previous  work  presented  thus  far  relies  on  the  IKF|/IEKFl  or  its  inverse,  the 
information  filter,  to  compute  an  estimate.  Fox  et  al.  ra  use  a  iPFl  to  perform  co¬ 
operative  localization.  They  represent  the  distribution  by  a  large  number  of  samples 
rather  than  as  a  Gaussian  distribution.  Section  13.4.21  outlines  the  advantages  of  the 
EE  over  the  EE  However  transferring  the  distribution  consisting  of  many  particles 
between  vehicles  requires  a  comparably  fast  communication  channel  which  is  avail¬ 
able  on  land  with  IRFl  communication ,  but  the  slow  acoustic  communication  channel 
limits  the  applicability  of  this  approach  underwater.  Additionally  Fox’s  approach  also 
requires  (half-)duplex  communication  such  that  broadcast-based  approaches  cannot 
be  used,  which  further  increases  the  necessary  bandwidth. 

3.5.2  Underwater  Vehicles 

Eickstedt  and  Schmidt  [29]  proposed  deploying  two  lAUVk  equipped  with  an  active 
sonar  in  which  the  sources  were  synchronized  to  a  global  clock  and  were  transmitting 
orthogonal  chirp  sequences.  This  enabled  one  vehicle  to  use  the  ping  emitted  by  the 
other  to  perform  acoustic  tomography  and  bi-static  target  detection.  While  no  data 
was  exchanged  through  the  acoustic  channel,  it  was  one  of  the  first  times  globally 
synchronized  transceivers  could  be  used  for  intra- vehicle  range  measurements  under¬ 
water.  Leonard  et  al.  m  and  Paley  et  al.  [69]  used  a  fleet  of  gliders  to  jointly  survey 
a  large  body  of  water.  When  on  the  surface,  the  gliders  connected  to  a  central  com¬ 
puter  and  offloaded  the  data  they  collected  during  their  last  dive  as  well  as  their 
actual  IGPSklerived  position.  The  data  and  the  position  information  was  used  by 
the  central  computer  to  calculate  an  individual  track  for  each  glider  which  allowed 
optimal  data  collection.  This  track  was  sent  to  the  glider  via  satellite  and  each  vehicle 
was  unaware  of  all  others.  No  data  was  exchanged  while  they  were  submerged. 

The  idea  for  ICNI  was  mostly  inspired  by  earlier  work  done  in  single-beacon  lo¬ 
calization.  Single-beacon  navigation  uses  a  single  ILBLI  beacon  instead  of  a  network 
of  three  or  more  as  shown  in  section  12.1.11  to  obtain  a  position  estimate.  While 
classic  ILBLI  provides  an  absolute  position  every  time  the  beacon  network  is  queried, 
single-beacon  navigation  requires  several  range  measurements  obtained  from  the  same 
beacon  combined  with  dead-reckoning  information  collected  between  range  measure¬ 
ments.  The  earliest  work  in  the  field  was  presented  by  Scherbatyuk  [76] .  He  used  a 
Least  Squares  QLSQD  algorithm  to  combine  three  or  more  range  readings  with  dead¬ 
reckoning  information  to  solve  for  a  position  in  the  horizontal  plane  while  the  depth 
was  provided  directly  through  a  pressure  gauge.  Vaganay  et  al.  used  an  IEKFI  similar 
to  the  one  presented  in  section  13.4.11  to  initially  solve  the  “homing  problem”  in  which 
a  vehicle  attempts  to  get  as  close  as  possible  to  a  point  which  is  marked  by  an  acous¬ 
tic  beacon  [87] .  Baccou  and  Jouvencel  later  applied  that  approach  to  general  vehicle 
navigation  0,0.  Another lEKFlbased  single  beacon  approach  called  Synthetic  Long 
Baseline  (SLBL)  was  presented  by  Larsen  [56] . 

Combining  the  concept  of  a  mobile  ILBLlbeacon  [24],  which  not  only  provides 
a  range  to  the  interrogating  vehicle  but  also  its  position,  with  the  idea  of  using 
consecutive  range  measurements  combined  with  dead-reckoning  information  led  to 
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Figure  3-6:  Setup  and  results  of  the  AOFNC2003  IMLBLI  experiment.  Top:  Setup. 
Two  boats  outfitted  with  ICPSI  and  ILBLI  beacons  and  an  IAIJVI  (with  IDYLI  and  com¬ 
pass)  interrogating  both  beacons.  Bottom:  ICPSltrack  of  the  boats  (ICNAk)  and  the 
lAIJVfs  position  estimate  using  an  IEKFI  Note  the  difference  between  the  estimated 
(x)  and  the  true  surfacing  position  (+). 


the  concept  of  Moving  Long  Baseline  (IMLBLI).  This  concept  was  first  expressed 
by  Vagany  et  al.  [88] .  They  used  a  single  IAIJVI  outfitted  with  an  AVTRAK  ILBLI 
interrogator  and  two  boats,  each  carrying  a  conventional  ILBLI  beacon  (figure  13-61) . 
The  IAIJVI  was  sent  on  a  pre-programmed  mission  and  interrogated  both  ship-side 
beacons  every  3  seconds.  While  the  lAUVl  logged  the  ITOF]/range.  the  boats,  serving 
as  beacons  vehicles,  logged  their  ICPSlderived  position  at  the  time  their  beacons  were 
interrogated.  By  combining  the  log-files  from  the  IAIJVI  and  the  two  boats,  the  pure 
IDR.I  track  from  the  IAIJVI  was  corrected  with  the  IMLBLI  algorithm .  The  algorithm 
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used  is  proprietary  and  no  information  on  how  the  range/position  pairs  were  fused 
with  the  IDRI  track  is  available. 


3.5.3  The  CN-Algorithm 

Due  to  the  shortcomings  of  the  two  classical  approaches  described  in  section  ITU  we 
propose  our  ICNI  algorithm.  The  ICNI  algorithm  uses  consecutive  range  measurements 
which  it  forward  propagates  to  align  in  time.  The  intersections  of  the  range  circles  are 
hypotheses  for  possible  positions.  By  combining  the  latest  range  measurement  with 
several  range  measurements  forward  propagated  from  the  past,  the  l(  INI  algorithm  ob¬ 
tains  a  number  of  hypotheses  for  the  I  ATI  Vis  position.  Using  available  dead-reckoning 
information  it  searches  for  the  most  consistent  path  through  the  sets  of  hypotheses 
generated  whenever  a  new  range  measurement  was  received.  Unlike  the  IEKFI  it  in¬ 
herently  maintains  several  hypotheses  about  its  position  which  are  all  reevaluated 
whenever  a  new  range  measurement  is  available.  This  allows  it  to  recover  after  a 
measurement  with  a  large  error  was  received.  The  individual  intersections  are  sim¬ 
ilar  to  the  particles  in  a  IPFI  in  that  they  represent  hypotheses  about  the  vehicle’s 
position.  Unlike  the  particles  in  the  IPFI  however  they  are  not  sampled,  but  represent 
the  outcome  of  an  intersection.  As  intersections  will  always  instantiate  hypotheses 
near  the  true  position  (assuming  a  bounded  error)  this  algorithm  will  not  suffer  from 
“particle  depletion”  close  to  the  correct  position  which  is  possible  with  the  IPFI  The 
ICNI  algorithm  works  as  follows. 

With  each  successful  transmission  at  time  k  the  AUV  receives  an  estimate  of 
the  CNA’s  position  xc{k)  =  [xc (k) ,  yc (k)]T ,  the  covariance  matrix,  Pc(k),  which 
accounts  for  the  confidence  the  CNA  has  in  each  component  of  xc(k),  a  depth  zc(k) 
and  the  range  r(k)  between  the  AUV  and  the  CNA. 


Pc(k) 


°CJ{k)  acxy\k) 
<%%)  <%\k) 


The  state  xc(k)  and  the  covariance  PC (k)  can  be  a  snapshot  from  the  navigation 
filter  running  on  the  CNA  or  from  the  GPS  in  the  case  that  the  CNA  is  at  the  sur¬ 
face.  The  range  r(k)  is  directly  obtained  by  the  AUV  through  the  IPPSl-svnchronized 
transmission  feature  (see  sectionl2.2.2jh  Many  experiments  have  shown  that  the  error 
in  the  range  measurement  r(k)  is  only  weakly  range-dependent  and  can  be  modeled 
as  a  Gaussian  with  mean  r(k')  and  a  fixed  variance  of. 

Furthermore,  the  AUV  builds  a  matrix  D  where  each  entry  D(n,m )  contains 
the  distance  traveled  df, ^  =  [(h^,  dynxn\ 1  between  receiving  a  transmission  at  t[n) 
and  at  t(m)  as  obtained  from  proprioceptive  measurements  as  well  as  the  covariance 
matrix  Q^jn  associated  with  that  measurement. 


Q 


n,m 


^dXfrm 


0 

2 

Gfo/rvm 


Figure  13-71  shows  how  the  AUV  uses  information  received  at  t(n)  and  t(m)  to 
compute  two  possible  solutions  for  its  position  at  t(m):  The  circle  with  radius  r(n) 
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Figure  3-7:  Computing  two  possible  positions  of  the  AUV  at  t(m)  (xA(m),yA(m)  and 
xA  (m),  yA(m))  using  the  dead-reckoning  information  dxn^n,  dyn^n  and  the  informa¬ 
tion  xc(n),yc(n)  and  r(n)  received  at  t(n)  from  CNAl  and  xc(m),yc(m)  and  r(m ) 
received  at  t(m)  from  CNA2. 


dehnes  all  possible  positions  at  t(n).  Shifting  the  center  of  this  circle  by  [dx^^n,  dyn^n]T 
(dashed  black  line  in  figure  13-71)  and  solving  the  resulting  quadratic  equation,  we 
obtain  a  set  XA{m )  of  0,  1  or  2  intersections  with  the  circle  around  xc(m)  with 
radius  r{m). 


XA(m )  =  T(x(n)c ,  x(m)c ,  r(n),r(m),  d^)  (3.17) 

with 


XA(m)  =  0  or  XA(m)  =  xA{m)  or  XA(m )  =  ^  ^{m) 

Using  other  values  for  n  (n  =  [1, . . .  ,m  —  1]),  we  can  compute  up  to  2 (m  — 
1)  solutions  for  xA(m).  For  the  upcoming  computations  we  assume  that  we  use  q 
solutions.  The  Jacobian  of  the  intersection  function  T  with  respect  to  the  measured 
and  transmitted  parameters  xc(n),  xc(m),  r(n ),  r(m),  is  Jn,m  and  can  be  used 
to  compute  PA{m )  the  covariance  of  xA(m).  PA{m )  is  given  by 


PA{m) 


°xx2(m)  °xy2(m) 

rfx2(m)  rfy2(m) 


T  C1  T1 

u  n,m'^rn,m*J  n,m 


(3.18) 


with 
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0  crr2(m)  0 
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0 

0 

0 
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0 

0 

0 
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and 


dxA(m)  dxA(m)  dxA(m)  dxA(m)  dxA(m)  dxA(m)  dxA(m)  dxA(m) 

dxc (n)  dyc (n)  dxc (m)  dyc  (m)  dr(n)  dr(m)  ddxn,m  ddyn,m 

dyA(m)  dyA(m)  dyA(m)  dyA(m)  dyA(m)  dyA(m)  dyA(m)  dyA(m) 

dxc  (n)  dyc (n)  dxc\rn)  dyc (m)  dr(n )  dr{m)  ddxn^m  ddyn,m 


All  possible  solutions  for  xf{m)  and  their  respective  covariances  P4  (m)  are  com¬ 
bined  into  a  matrix  S(m),  where  v  is  the  index  for  all  solutions  at  time  t{rn). 
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We  also  define  a  position  matrix  T(m  —  q)  which  stores  all  possible  past  positions 
of  the  AUV,  going  back  to  t(m  —  q),  i.e.  xf(m  —  o)  ;  \/u  =  [1 . . .  q],  o  =  [1 . . .  q], 
their  respective  covariances  P4  (m  —  o)  and  an  associated  accumulated  transition 
cost  cu(m  —  o )  at  t(m  —  o),  where  a  indexes  all  possible  positions,  covariances  and 
costs  at  t(m  —  o). 


72 


Chapter  3.  Cooperative  Localization 


T(m  —  q ) 
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If  a  known  position  ar4(0)  (obtained  on  the  surface  through  GPS)  is  available  in  the 
beginning  it  can  be  used  to  initialize  T( 0)  =  [ccA(0)  c(0)  =  0].  If  no  initial  position  is 
available,  the  first  set  of  solutions  S(0)  initializes  T( 0)  and  position  estimates  become 
available  when  subsequent  information  packages  are  received. 


Our  cost  function  CUtV(m  —  o,m )  computes  the  cost  (inverse  of  likelihood)  of  the 
AUV  having  traveled  from  x^(m  —  o )  to  x^(m)  given  x^(m  —  o ),  P^(m  ~  o),  x^(m), 

p  ^  ( rii  ]  f  j -  r) — 

j.  y  y  i  I  v  j  \Ajjn-o  m  *  m— o,m  ‘ 


This  cost  is  expressed  by  the  “distance”  between  (x^(m  —  o)  +  dm_o  m),  a  solution 
at  t(m  —  o)  forward  propagated  by  the  dead-reckoning  information  dm_om) .  with  the 
associated  covariance  (P„  (ra  —  o)  +  Qm_0>m)  and  x ^  a  solution  at  t(m)  with  the  as¬ 
sociated  covariance  P;4.  The  distance  metric  used  is  the  Kullback-Leibler  divergence 
given  by 
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det(P*(m  -  o)  +  . 

+  trace((P^)-l(P*{m  -  o)  +  Q^^j)  + 

+  {xv  ~  (xt(m  ~o)  +  d^^))T (P^)_1 

(xv  ~  (xt(m  ~  o)  +  d sr:^))  -  2^ 


(3.19) 


Using  EUni  we  now  compute  the  total  cost  cUtV(m  —  o,m )  by  computing  the  cost 
CU)V  (m  ~  o,  m)  for  all  q 3  possible  transitions  from  T(m  —  q)  to  S(m)  and  adding  the 
new  transition  cost  CU}V{m  —  o,  m)  to  the  accumulated  cost  cu{m  —  6). 


cu,v(m  —  °, m )  =  CU:V(m  —  o ,  m)  +  cu(m  —  o )  V  u  =  [1 . . .  q],  v 

We  then  form  a  new  position  matrix  T(m) 


[l...q\,o=[l...q] 

(3.20) 


T(m )  = 


xf(m)  yf(m)  a?xx2(rn)  2(m)  <2(m)  Ci(m) 


Xvi™)  Vvi™)  °VXX  (m)avxy  (m)avyx  (™)  A,  (m) 


vt(m)  aL  (m)  ^  (m)  ^  M  (m)  c?M . 


Qyx 


Qyy 


=  [l...q] 


where  cv(m)  is  the  smallest  accumulated  cost  associated  with  the  transition  to 
solution  x^(m)  from  of  all  q2  possible  positions  x^(m  —  o ). 

cv(m)  =  mm(cUtV(m  —  o,m))  V  v  =  [1 . . .  q],  o  =  [1 . . .  q],  (3-21) 

All  solutions  x^(m)  are  now  hypotheses  for  possible  positions  of  the  AUV  at 
t(m)  and  weighted  by  the  associated  accumulated  transition  cost  cv(m).  The  likeliest 
position  x^(m),  i.e.  our  computed  solution  for  t(m),  is  the  one  with  the  smallest 
accumulated  transition  cost 


x^(m)  with  w  s.  t.  cw(m)  =  min (c„(m)) 

Wv 


(3.22) 


3.5.4  Example 

A  single  iteration  of  algorithm  [5]  is  shown  in  the  following  example.  Figure  13-81  shows 
a  snapshot  at  t(33)  during  a  cooperative  navigation  experiment.  The  AUV  (here 


74 


Chapter  3.  Cooperative  Localization 


9 

10 

11 

12 

13 

14 

15: 


Initialize  position  matrix  T( 0)  =  [cc^(0)  c(0)  =  0] 
loop  {compute  position} 
m  +  + 

Wait  for  new  range/position  pair  xc (m),zc (m),Pc (m),r(m)  from  CNA 
Use  zc(m )  to  project  xc(m )  to  a  plane  at  the  AUV’s  depth  zA(m ) 
for  j  =  1  to  q  do  {Calculate  intersection  solution  between  now  (m)  and  j  steps 
in  the  past} 
n  =  m  —  j 

xj  {jTl)  Q3-17D  \x(n)c ,x(m)c ,r(n),r(m),d^fn  {Position} 
pf(m)  =  J n,mGn,mJltm  {Covariance} 

S(rn)  <—  xf(m),  PA{m)  {Add  solution  xf(m)  and  its  covariance  Pf(m)  to 
solution  matrix:} 

end  for 

for  o  =  1  to  q  do  {Iterate  through  all  past  time  steps} 
for  u  =  1  to  q  do  {Iterate  through  all  positions} 
for  v  =  1  to  q  do  {Iterate  through  all  solutions} 

cu,v(m  —  o,  m)  cu(m  —  o)  +  (13.191) 


xA(m  -  o),PA(m  -  o),xA(m), 
P r  dm— o,mi  Qm^a^n 

16:  end  for 

17:  lxi(m)  P*{m)  c„(m)\ 

18:  end  for 

19:  end  for 

20:  The  computed  position  at  t(m)  is  :  xA(m)  =  xA{m)  with  w  s.  t.  cw(m )  = 

minv,;  (cv(m)) 

21:  end  loop 


Algorithm  5:  The  Cooperative  Navigation  (ICND  algorithm. 


simulated  by  an  ASC  which  also  provides  GPS  for  ground-truth)  has  just  received 
a  position/range-pair  from  the  CNA  (full  circle).  This  circle  intersects  with  the 
position/range-pair  received  at  £(32)  (dashed  circle)  and  forward  propagated  by  the 
dead-reckoned  distance  t°  *c(32/).  It  also  intersects  with  other  position/range- 
pairs  received  at  t(k),  (1  <  k  <  32)  (positions  of  CNA  not  shown)  forward  propagated 
to  xc(k')  by  the  corresponding  dead- reckoned  distance  All  intersections  and 

therefore  possible  solutions  at  £(33)  are  shown  with  their  corresponding  accumulated 
transition  cost.  The  inset  in  figure  IT71  shows  the  detailed  view  near  the  ground-truth 
(GPS)  position.  The  computed  position  at  £(33)  (marked  with  a  large  ”X”)  is  the 
one  with  the  smallest  accumulated  transition  cost  selected  out  of  all  possible  positions 
£C^(33).  In  this  case  it  is  not  the  one  closest  to  the  GPS-derived  position. 

The  complexity  to  compute  a  single  position  is  0(q 3)  where  q  is  the  number  of  past 
measurements  taken  into  account.  The  maximum  frequency  at  which  this  computa¬ 
tion  step  is  invoked  is  limited  by  the  duration  of  a  data  packet  transmission.  As  the 
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Figure  3-8:  All  possible  solutions  for  solution  #33  with  accumulated  transition  cost; 
Inset:  Detailed  view  of  selected  solution  and  GPS  ground-truth. 


transmission  of  a  data  packet  takes  10  s  the  highest  frequency  at  which  algorithm  [5] 
is  called  is  fmax  =  0.1  Hz.  For  q  «  10  the  time  to  compute  a  new  position  is  t=0.1  s 
on  a  1  GHz  PC.  This  makes  this  algorithm  well  suited  to  run  on  the  computer  of 
today’s  AUVs. 


3.6  Maintaining  Consistency 

When  robot  1  uses  the  position  estimate  of  robot  2  to  update  its  own,  their  position 
estimates  become  dependent.  This  can  be  observed  in  the  evolution  of  the  covariance 
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matrix  from  Plj2(fc  +  1)  to  Pi;2(/c  +  1)  during  the lEKFl update  (eq.  (13.141)1.  In 


Pi)2(fc  +  1) 


Pi{k  + 1)  Pi2(fc  +  1) 
P2i(fc  +  1)  P2(fc  + 1) 


the  elements  P\2{k  +  1)  and  P2i(k  +  1),  which  were  0  in  equation  (13.121).  indicating 
that  the  two  positions  were  not  dependent,  become  non-zero  meaning  that  the  two 
positions  are  now  dependent. 

If  at  a  later  time  robot  2  uses  the  information  from  robot  1  to  update  its  own 
position,  P2)i  (k  +  1)  in  (13.121)  is  not  longer  a  diagonal  matrix.  If  the  off-diagonal 
elements  are  then  assumed  to  be  0,  P2,i{k  +  1),  and,  after  the  update  P2) i(k  + 
1),  will  not  properly  represent  the  uncertainty  of  the  position  estimate  ii2[k  +  1). 
The  IEKFI  will  become  overconfident  in  the  estimate  of  yu2  and  can  diverge  [2j .  In 
section  13.6.51  we  show  an  example  of  lEKFlbased  ICNl  in  which  the  correlations  are  not 
taken  into  account  and  lead  to  an  overconfident  position  estimate.  Properly  modeling 
the  correlations  and  determining  the  correct  values  for  P\2{k  +  1)  and  P2\{k  +  1) 
requires  robot  1  to  have  exact  information  about  the  evolution  of  x2  which  is  difficult 
to  achieve  in  a  de-centralized  system. 

Various  approaches  have  been  devised  to  either  properly  account  for  the  corre¬ 
lation  or  to  use  very  conservative  uncertainty  bounds  to  avoid  overconfidence,  but 
many  of  the  algorithms  impose  additional  requirements  which  make  them  unfeasible 
for  many  CN-scenarios. 


3.6.1  Covariance  Intersection 

A  general  approach  to  the  problem  of  fusing  dependent  estimates  has  been  proposed 
by  Julier  and  Uhlmann  [49J ,  [50j.  Their  Covariance  Intersection  (Cl)  algorithm  fuses 
two  different  estimates  for  a  random  variable,  each  represented  by  their  estimated 
mean  and  covariance  much  like  the  update  step  in  the  Kalman  filter.  The  result  is  a 
posterior  covariance  that  guarantees  consistency  under  the  assumption  of  Gaussian 
noise.  Arambel  et  al.  present  an  application  of  the  Cl  algorithm  for  a  group  of 
space  vehicles,  in  which  relative  position  measurements  are  communicated  in  a  ring 
topology  [2] .  Each  of  these  works  has  examples  of  how  the  state  estimator  can  diverge 
if  estimates  are  fused  with  a  simple  Kalman  update  without  accounting  for  correlation 
among  the  estimates. 

A  disadvantage  of  the  Cl  algorithm  is  that  it  can  only  fuse  two  state  estimates. 
Additionally,  unlike  the  standard  Kalman  Filter,  it  cannot  perform  a  partial  update 
such  as  those  that  apply  to  vehicles  that  only  have  a  range  or  bearing  sensor.  As  a 
result,  robots  that  only  have  a  bearing  sensor,  such  as  a  monocular  camera,  or  have 
only  range  information  from  time-of-flight-based  techniques  cannot  participate  in  a 
setup  which  relies  on  Cl  for  the  update  of  position  estimates. 
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3.6.2  The  IIUI  Algorithm 

We  propose  the  IIUI  algorithm  which  ensures  that  position  estimates  do  not  become  de¬ 
pendent  as  outlined  in  the  previous  section.  The  IIUI  algorithm  requires  that  each  robot 
includes  additional  information  within  its  status  broadcast.  The  receiving  robots  can 
then  use  this  information  to  ensure  that  the  correlations  are  properly  accounted  for. 
The  approach  does  not  require  centralized  data  storage  and  processing  as  all  updates 
are  done  locally  on  each  vehicle  using  only  data  from  the  broadcasting  vehicle.  It 
does  not  enforce  a  particular  communication  hierarchy  or  topology  and  individual 
members  can  join  and  leave  the  group  and  do  not  need  any  awareness  of  previous 
communications  or  the  size  of  the  group.  In  contrast  to  other  methods,  broadcasts 
within  the  IIUI  algorithm  framework  do  not  need  to  be  received  by  all  participating  ve¬ 
hicles  as  each  transmission  contains  all  the  information  that  is  required  for  a  position 
update  which  accounts  for  the  correlations. 

Concept 

The  basic  concept  of  the  algorithm  is  to  maintain  a  filter  for  each  subset  of  vehicles 
from  which  it  received  updates  and  ensure  that  only  information  from  these  vehicles 
is  used  to  update  that  particular  filter.  During  broadcast  the  transmitting  vehicle 
sends  the  estimates  from  all  filters.  On  a  receiving  vehicle  the  IIUI  algorithm  matches 
the  results  from  the  filters  of  the  transmitting  vehicles  to  its  own  bank  of  filters  and 
updates  them  accordingly.  It  is  important  to  note  the  that  the  El]  algorithm  does 
not  do  the  update  itself,  but  is  an  information  arbiter.  The  algorithm  is  thus  able  to 
work  with  any  type  of  Bayes  filter.  Figure  ITfil  compares  the  two  approaches  for  a  set 
of  three  vehicles. 

Initialization 

For  the  |TU]  algorithm,  each  vehicle  i  now  maintains  a  set  Xt(k)  of  state  estimate 
vectors  together  with  a  set  'Pl{h)  of  associated  covariance  matrices.  As  we  will  explain 
later,  the  maximum  size  of  the  set  is  2n  where  n  is  the  total  number  of  vehicles 
cooperating  for  navigation. 

Xi  (k)  =  {*?(*),.  w} 

vm  =  {pi(k),...,puk),...,pr(k)} 

Before  vehicle  i  receives  information  from  any  other  vehicle  the  only  contents  of 
Xi(k )  and  Vi{k)  are  x\(k)  and  P]{k). 


Broadcast  Receive 
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Figure  3-9:  Comparison  between  the  standard IEKFI  (left)  and  the [TU] update  (right). 
In  both  cases  vehicle  1  (gray  box)  receives  a  broadcast  from  vehicle  2  and  broadcasts 
information  to  vehicle  3.  Left:  The  standard  IEKFI  maintains  a  single  Liter  which  is 
updated  by  all  other  vehicles  and  therefore  dependent  to  all  other  vehicles.  Right: 
The  Ell  algorithm  maintains  a  filter  for  each  subset  of  vehicles  such  that  there  is  always 
a  filter  which  is  not  dependent  to  at  least  one  vehicle  and  broadcasts  all  estimates. 


Prediction 

Each  time  vehicle  i  receives  proprioceptive  sensor  readings  it  uses  the  Kalman  Filter 
prediction  steps  for  state  and  covariance  (eq.  (13.91)  and  (13.101)1  to  propagate  x\{k) 
and  P\  (k). 


xj(k)  x](k  + 1) 

P}(k)  P)(k  + 1) 


First  Update 

When  vehicle  i  receives  a  broadcast  from  vehicle  j  at  time  l,  it  first  instantiates  a 
second  filter  by  copying  the  state  and  covariance  matrix. 
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x2S)  =  x|(() 

p2,(‘)  =  p'M 

The  vehicle  also  instantiates  a  matrix  T,  in  which  each  row  represents  a  filter  and 
each  column  represents  a  vehicle  number.  The  entry  in  matrix  Ti(q,i )  is  the  time 
when  vehicle  i  was  last  used  to  update  filter  q. 

Using  the  Kalman  update  equations  (eq.  (13.1311  and  (13.1411 ).  we  now  only  update 
x2(l)  and  Pi  ( / 1  After  this  update,  our  sets  A’ , i/j .  "PS  / ,  ,,i , ,  |  the  matrix  T,  (7)  are 


A’.(i) 

?\(0 


T.(i) 


{pJ(Z),.P?(Z)} 

To  ...  I  0 
[  0  ...  I  0 

T 


0  0  ...  0 
l  0  ...  0 

T 

j 


Subsequent  Predictions 


The  first  prediction  for  vehicle  i  after  the  update,  propagates  both  filters  using 
eq.  (13.911  and  (13.1011  to  Xi(l  +  1)  and  7 °i(l  +  1)  and  all  elements  in  column  i  in 
Ti(l  +  1)  are  set  to  l  +  1. 


Ti(Z  +  l) 


0  ...  Z  +  l  0  ...  0  0  ...  0 

0  ...  Z  +  l  0  ...  I  0  ...  0 

T  T 

*  3 


Matrix  T,  therefore  keeps  track  of  which  vehicles  have  been  used  to  update  a  partic¬ 
ular  filter  as  well  as  the  age  of  the  updates.  Predictions  after  l  +  1  up  to  the  next 
update  are  propagated  the  same  way,  both  filters  are  propagated  and  all  elements  in 
column  i  of  Tr  are  set  to  the  actual  time.  All  other  columns  remain  unchanged. 


Broadcast 

Every  time  vehicle  i  sends  out  a  broadcast,  the  transmitted  data  consists  of  A’,,  "P, 
and  T,  .  By  maintaining  a  state  x]  on  vehicle  i  which  is  continuously  propagated  and 
has  not  been  updated  with  information  from  vehicle  j,  we  make  sure  that  a  future 
broadcast  from  vehicle  i  received  by  vehicle  j  contains  a  state  which  is  not  dependent 
with  vehicle  j  and  can  therefore  be  used  by  vehicle  j  for  an  update. 
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Subsequent  Updates 

The  general  update  case  in  which  vehicle  i  receives  a  broadcast  from  j  after  both 
vehicles  have  received  broadcasts  from  various  other  vehicles  and  have  incorporated 
those  to  update  their  navigation  filters  unfolds  as  follows. 

We  define  Si  as  the  set  of  all  m  vehicle  ids  from  which  vehicle  i  received  updates. 
Si  not  only  contains  the  ids  of  from  which  vehicle  i  has  directly  received  broadcasts, 
but  also  those  ids  which  have  been  propagated  to  it  through  other  vehicles.  The 
power  set  2Si  then  contains  all  2m  possible  subsets  of  these  ids.  Each  subset 

(3.23) 

then  corresponds  to  a  filter  maintained  in  which  maintains  a  state  that  has 

been  updated  by  the  ids  in  the  corresponding  subset  A,  and  therefore  has  correlations 
with  these  vehicles.  The  information  about  which  ids  are  in  the  individual  subsets  is 
maintained  in  line  q  of  T,  as  each  line  in  Tt  corresponds  to  a  subset  of  A- 

Similarly  there  is  a  set  Sj  for  all  o  ids  from  which  vehicle  j  has  received  broadcasts. 

c(2*uj) 

When  vehicle  i  receives  Xj,Pj  and  T:i  from  vehicle  j  it  first  adds  entries  in  X.Pl 
and  T,  for  all  elements  of  Aj  which  are  not  in  A-  As  a  result  vehicle  i  then  maintains 
filters  for  a  new  set  •A 


AiUAj—>Ai 

Each  filter  x'j , P'[  represented  by  A'  is  now  updated  without  introducing  any  addi¬ 
tional  correlations.  This  means  that  A  =  A[.  To  update  aif  ,Pj  we  now  find  all 
possible  combinations  of  sets  from  A  and  A,  such  that 

A°U  A^A  (3.24) 

Each  of  these  combinations  represents  a  possible  update  for  a^Pj 


—a  with  set 


X; 


—  ;  lETUll  with  pq 


(3.25) 

(3.26) 


We  now  select  g  and  h  such  that  Pcj  has  the  smallest  trace  of  all  possible  combinations. 

(3.27) 

Using  g*  and  h*  determined  through  eq.  (13.271).  we  use  eq.  (13.251)  to  update  the  state. 


(g*,h*)=  argmin  I  trace{Pl 

g,h  s.t.  (13.2411  ^ 


_2*  (T37T31  with  x^* 
^ 


x ; 


Line  q  in  T,  is  updated  to  reflect  the  age  of  updates. 
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r?(i,  u) 

=  TT(g*,u) 

V«  e  A f 

=  Th;(h*,u ) 

Vw  G  Ahj* 

All  steps  in  section  (13.6.21)  are  repeated  for  all  2n  filters  on  vehicle  i,  and  all  other 
vehicles  that  overheard  the  broadcast,  update  their  local  filters  accordingly. 

3.6.3  Enforcing  Constant  Set  Size 

The  amount  of  information  which  needs  to  be  transmitted  during  each  broadcast,  as 
well  as  the  number  of  local  prediction  and  update  steps,  grows  with  0(m2)  where 
m  is  the  size  of  set  St  as  defined  in  section  13.6.21  The  amount  of  data  which  needs 
to  be  transmitted  per  filter  however  is  fairly  small  if  the  state  is  parameterized  with 
a  mean  and  a  covariance  (~  10  bytes)  and  the  update  of  each  filter  only  requires  4 
[2  x  6]  •  [6  x  6]  matrix  multiplications  for  a  3D  environment  where  range  and  heading 
measurements  are  available.  Assuming  a  data  packet  size  of  lOkBytes,  set  sizes  up 
to  30  ids  are  feasible. 

For  a  large  group  of  cooperating  robots  with  the  same  level  of  uncertainty  in  their 
proprioceptive  measurements,  Roumeliotis  et  al.  show  in  mi  that  the  uncertainty 
growth  is  inversely  proportional  to  the  number  of  robots.  Thus  the  contribution  of 
each  additional  robot  follows  a  law  of  diminishing  return.  This  suggests  that  set  sizes 
of  30  and  less  are  sufficient  to  obtain  an  improvement  of  navigation  accuracy  which  is 
close  to  the  theoretical  maximum  obtained  when  broadcasts  of  all  available  vehicles 
are  incorporated. 

Based  on  our  available  communications  bandwidth  and  available  processing  cycles 
we  can  choose  an  upper  bound  b  for  the  size  of  St.  If  our  set  size  grows  larger  than 
b  we  can  incorporate  the  new  broadcast  according  to  section  13.6.21  and  then  resize 
Si  by  eliminating  the  id  which  contributes  the  least  amount  of  information.  The 
resize  process  then  consists  of  two  steps.  First  we  determine  the  vehicle  (id)  which 
contributes  the  least  amount  of  information.  Second  we  remove  this  id  from  Si  and 
modify  X/P,  and  T*  accordingly. 

Compare 

One  method  to  determine  the  vehicle  with  id  q  which  contributes  the  least  amount 
of  information  is  to  compare  the  trace  difference  between  the  filter  that  was  only 
updated  by  {q,i}  with  the  filter  that  has  the  dead  reckoning  result  only  {/'}. 


Q 


* 


argmin  ( trace  (Pf) 
q  ' 

VqeSi,q^i 
p9i  s.t.  Ag  =  {i,q} 
P’1  s.t.  Ah  =  {i} 


t/  UCf:  (  p’i  j  j 
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Table  3.1:  Contents  of  X ,"P  and  T  at  time  k  =  1 


Ti(l) 

*i(l) 

T  i(l) 

Ti(  1) 

*2(1) 

TM 1) 

r3(i) 

*3(1) 

■Ps(l) 

i — 1 

lfc_ 

*4(1) 

1 — 1 

1 

*l(l) 

^(1) 

1 

*2(!) 

U(D 

1 

*3(!) 

U(D 

1 

x\(l) 

U(  i) 

Eliminate 

After  we  determine  q*  we  remove  all  filters  depending  on  q*  from  our  sets  X^P,  and 
obtain  our  new  sets  X~  and  P~  and  our  updated  matrix  by  removing  all  lines 
which  have  a  non-zero  entry  in  column  q*. 


x, 

if  g*fAh 

XT 

(3.28) 

Vl 

P’l  if  q*<£Ah 

V- 

(3.29) 

T, 

Ti(g,h)  Vg,  with  Ti(g,q*)= 0 

T~ 

(3.30) 

3.6.4  Example 

The  four  frames  in  figure ITTOl and  the  tables  I  through  IV  show  how  the  sets  Xl.Pl 
and  the  matrix  T,  evolve  over  time. 


k=l 

Up  to  this  point  all  four  vehicles  have  only  used  dead-reckoning  information  so 
none  of  their  positions  are  dependent.  All  sets  Xi^Pi  only  contain  a  single  state 
and  covariance  matrix. 

k=2 

Vehicle  1  broadcasts  its  state  aq(2)  which  is  received  by  vehicle  2  and  3.  Both 
vehicles  instantiate  a  second  filter  X2(2),Pl(2)  and  x\{2)1P\(2)  respectively 
which  are  updated  with  the  broadcast  and  range  received  from  vehicle  1,  while 
the  other  filter  in  both  vehicles  are  not. 

k=3 

Up  to  k=3  all  filters  (filter  1  in  vehicle  1  and  4,  filter  2  in  vehicle  2  and  3)  are 
propagated  using  the  Kalman  time  prediction  step.  At  k=3  the  broadcast  from 
2  is  received  at  4.  As  2  has  been  previously  updated  with  1  the  set  of  filters 
received  by  4  contains  2  new  ids  (1  and  2).  Vehicle  4  therefore  instantiates  3 
additional  filters,  each  containing  a  possible  permutation  of  Ai  as  specified  in 
eq.  dS23D- 
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Figure  3-10:  Four  vehicles  exchanging  navigation  information  for  Cooperative  Navi¬ 
gation  from  time  k=l  (top  left)  to  k=4  (bottom  right).  The  arrows  indicate  which 
vehicle  broadcasts  during  a  particular  time  step  and  which  vehicles  received  the  broad¬ 
cast.  Below  each  vehicle  are  the  states  which  were  used  to  update  this  vehicle’s  various 
position  filters. 


Table  3.2:  Contents  of  X  and  T  at  time  k  —  2 


At  k=4  vehicle  3  receives  a  broadcast  from  vehicle  4.  After  the  update  vehicle 
3  now  maintains  the  maximum  set  of  8  filters. 


3.6.5  Simulation  Results 

To  evaluate  the  algorithm  we  set  up  a  simulation  with  three  vehicles.  All  three 
vehicles  had  a  very  accurate  initial  position  estimate.  After  the  start  of  the  mission 
they  continuously  estimated  their  positions  using  forward  (u)  and  starboard  (v)  speed 
as  well  as  heading  measurements  ( 6 )  from  very  noisy  sensors.  The  sensor  noise  was 
assumed  Gaussian  and  the  standard  deviations  are  show  in  table  13.51  Using  these 
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Table  3.3:  Contents  of  X ,"P  and  T  at  time  k  =  3 


Table  3.4:  Contents  of  X .'P  and  T  at  time  k  —  4 


T2(4) 

*2(4) 

7M  4) 

4 

*2(4) 

Pfo) 

24 

*1(4) 

J^(4) 

T  i(3) 

"7* - - 

T— 1 

1* 

v  i(4) 

4 

*1(4) 

P\(  4) 

T„(4) 

*4  (4) 

^4(4) 

4 

*i(4) 

2*1(4) 

2 

4 

*1(4) 

4) 

3 

4 

*1(4) 

^(4) 

23 

4 

*1(4) 

P\{  4) 

13(4) 

*3(4) 

^3(4) 

4 

*1(4) 

E(4) 

2 

4 

*1(4) 

^(4) 

3 

4 

*1(4) 

^(4) 

2 

3 

4 

*1(4) 

P\{  4) 

4 

4 

*1(4) 

Pfo) 

2 

4 

4 

*1(4) 

A  (4) 

3 

4 

4 

*1(4) 

pI(  4) 

2 

3 

4 

4 

*1(4) 

^3(4) 

sensors  each  vehicle  dead-reckoned  its  position  using  the  IEKFI  prediction  equations 
for  state  and  covariance  (13. 91). (13. 91). 

Table  3.5:  Variance  of  sensor  noise  for  the  simulated  vehicles. 


Vehicle 

( Jr 

1 

0.3  m/s 

10° 

0.1  m 

2 

0.3  m/s 

10° 

0.1  m 

3 

0.2  m/s 

2° 

0.1  m 

Figure ETTT1  shows  two  typical  runs.  Past  true  positions  are  indicated  by  small  “x” 
and  the  actual  true  position  at  t=2000  s  with  a  large  “x”.  The  vehicle’s  position 
estimate  is  indicated  by  a  large  “+”  together  with  the  3cr-covariance  ellipse.  The 
enlarged  sections  in  the  insets  show  clearly  how  for  all  three  vehicles  the  true  position 
is  outside  3cr-bound,  i.e.  the  99.6  %  confidence  interval  when  using  the  standard 
lEKFlupdate  (figure  13-111  left),  but  well  within  it  when  using  the lEKFl  with  the IIU1 
algorithm  for  the  update  (figure [Til] right). 

Figure  13-121  shows  how  the  the  error  x  in  the  position  estimate  for  the  x  component 
develops  over  time.  As  no  outside  position  fix  is  available  to  any  of  the  three  vehicles 
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Figure  3-11:  True  (x)  and  estimated  (+)  position  with  3cr-covariance  ellipse  of  three 
vehicles  navigating  cooperatively  using  anIEKFIand  naively  incorporating  all  updates 
(left)  or  selectively  updating  using  the IIUI algorithm  (right).  Note  the  change  of  scale 
between  of  the  enlarged  sections  on  the  left  and  on  the  right  plot. 


the  error  grows.  The  selective  updating  used  in  the  HlJl- algorithm  incorporates  fewer 
measurements  than  the  standard  IEKFI  and  as  a  result  the  error  grows  faster,  but  the 
error  accumulated  through  the  11171  algorithm  is  properly  accounted  for  and  the  error 
x  remains  well  within  the  3  ax,,. -bound  (figure  13-121  right)  while  the  standard  IEKFI 
algorithm  is  overconfident.  This  overconfidence  can  cause  the  standard  IEKFI- filter  to 
diverge. 

To  asses  the  consistency  of  the  position  estimate  of  standard  a  IEKFI  vs.  that  of 
the  IIUI  algorithm  we  computed  the  Normalized  Estimation  Error  Squared  fINEESI)  as 
described  in  [10j  for  20  runs  (ten  standard  IEKFI  and  ten  17171) . 

e(k)  =  x{k\k)T  P(k\k)~ 1  x{k\k) 

For  each  time  k  we  compute  the  N  =  10  average  INEESI  e(k) . 

2=1 

Under  the  hypothesis  H0  that  the  filter  is  consistent  and  under  the  linear- Gaussian 
assumption  Ne(k )  will  have  a  chi-square  density  with  N  nx  degrees  of  freedom,  where 
nx  is  the  dimension  of  x.  The  hypothesis  H0,  that  the  state  estimation  errors  are 
consistent  with  the  filter-calculated  covariances,  also  called  chi-square  test,  is  accepted 
if 
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Figure  3-12:  Error  in  x-position  (blue)  and  3 a  bound  (red)  of  vehicle  1  navigating 
cooperatively  using  anIEKFIand  naively  incorporating  all  updates  (left)  or  selectively 
updating  using  the IITJI algorithm  (right). 


Figure  3-13:  Averaged INEESI for  10  runs  as  shown  in  fig  13- 111  using  the  standard  IEKFI 
(left)  and  the  IEKFI  with  the  ITT Jl  algorithm  (right). 


e(k)  E  [r i, r2] 


where  the  acceptance  interval  is  determined  such  that 
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P{e(k)  e  [ri,r2]  \H0}  =  1-a 

The  two  sided  95  %  region  for  a  20  degree  of  freedom  (Nnx  =  10  •  2  =  2  =  20) 
chi-square  distribution  is 

[X2o(0-025),  X2o(0-975)]  =  [9.60,  34.2]  (3.32) 

Dividing  the  interval  in  (13.321)  by  N  we  obtain 

xio(0-025)  xjp (0.975) 

N  ’  N 

Figure |3zI31  shows  the  10-run  average  INEESI  according  to  (13.311)  and  the  boundaries 
determined  in  (13.331) .  For  the  standard  IEKFI  update  the  INEESI  quickly  grows  above 
the  upper  bound  (figure  ITT31  left)  and  indicates  that  this  approach  not  only  leads 
to  inconsistent  results,  but  that  this  inconsistency  is  growing.  For  the  IITJI  algorithm 
between  5  %  and  9  %  of  the  values  fall  outside  the  95  %  region  which  is  acceptable  [10] . 

3.6.6  Conclusion 

Simulations  such  as  the  one  in  section  13.6.51  show  that  not  properly  accounting  for  the 
correlations  between  vehicles  can  quickly  cause  the  navigation  filter  to  diverge.  Our 
proposed  Uni  algorithm  ensures  that  a  filter  is  only  updated  with  a  measurement  that  it 
is  not  dependent  to,  thus  ensuring  that  the  position  estimate  of  all  individual  filters 
will  not  suffer  from  overconfidence.  We  also  propose  a  method  that  maintains  an 
upper  bound  on  the  additional  cost  in  computation  and  communication  bandwidth. 


=  [0.96, 3.42] .  (3.33) 
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Intra- Vehicle  Geometries  for 
Cooperating  I  AT  J  Vis 


When  a  heterogeneous  group  of  vehicles  exchanges  navigation  information  two  sce¬ 
narios  are  possible.  In  the  first  case,  every  vehicle  broadcasts  information  and  every 
other  vehicle  receiving  the  message  incorporates  it  to  improve  its  own  position  esti¬ 
mate.  Each  vehicle  can  at  any  point  transmit  navigation  information  or  receive  and 
incorporate  it.  We  refer  to  this  scenario  as  “organic  cooperation”  and  it  is  outlined 
in  section  14.1.11  The  second  one,  described  in  section  14.1.21  is  a  more  hierarchical 
approach  in  which  a  dedicated  set  of  Communication  and  Navigation  Aid  (1CNAI) 
vehicles  exists.  These  vehicles  maintain  a  very  accurate  position  estimate  throughout 
the  entire  mission  and  broadcast  it  so  that  all  other  vehicles  may  use  this  information 
to  improve  their  position  estimate. 


In  both  scenarios,  the  achievable  improvement  of  the  position  estimate  strongly 
depends  on  the  relative  position  of  the  participating  lAUVb  with  respect  to  each 
other.  However  only  in  the  hierarchical  scenario  in  which  dedicated  beacons  in  the 
form  ofICNAb  exist,  this  geometry  can  be  controlled  by  positioning  the  ICNAh  This 
is  illustrated  in  a  simulation  in  section  14.2.11  Before  introducing  an  algorithm  for 
dynamic  positioning  ofICNAb  to  minimize  the  navigation  uncertainty  of  all  receiving 
vehicles,  in  section  14.3.61  we  will  first  discuss  previous  work  related  to  error  metrics  in 
14.3.21  Various  error  metrics  which  can  be  used  to  parametrize  the  position  uncertainty 
are  shown  in  section  14.3.31  To  address  the  problem  of  dynamically  planning  optimal 
beacon  positioning  we  briefly  investigate  the  case  of  simultaneous  trilateration  from 
static  beacons  in  section  14.3.41  Section  14.3.51  explains  the  assumptions  made  by  our 
algorithm  in  controlling  the  ICNAb  and  14.3.71  presents  the  results  for  two  simulated 
runs. 
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Figure  4-1:  Organic  ICNI  scenario:  IAIJVI 1  and  IAUVI 2  use  “beacons  of  opportunity” 
such  as  a  recently  submerged  glider  which  still  has  a  very  good  dead-reckoned  estimate 
or  a  recharging  solar  IAIJVI  or  a  communications  buoy  which  both  have  access  toIGPSl 


4.1  Vehicle  Task  Hierarchies  for  ICN 

4.1.1  Organic  Cooperation 

Figure  03]  shows  a  possible  scenario  of  a  heterogeneous  group  of  vehicles  in  which  all 
vehicles  can  at  one  point  in  time  participate  actively  (by  transmitting)  or  passively 
(by  receiving)  in  cooperative  navigation.  For  example  a  solar  powered  IAIJVI  such 
as  the  one  described  by  Blidberg  m  and  Crimmins  [221,  can  serve  as  a  navigation 
beacon  while  recharging  its  batteries  on  the  surface  while  also  having  access  to  IGPSI 
A  glider  (see  section  ll.l.ll)  as  described  by  Eriksen  in  [30]  penetrates  the  surface  at 
the  end  of  each  dive  to  obtain  a  IGPSI  fix,  as  well  as  to  communicate  over  satellite. 
During  its  time  at  the  surface  with  access  to  IGPSI  the  glider  can  serve  as  a  navigation 
beacons=  for  submerged  vehicles  in  the  area. 

4.1.2  Dedicated  Navigation  Beacon  Vehicles 

The  concept  of  dedicated  ICNAI  was  first  proposed  in  [88  J  for  a  mine-hunting  scenario 
(shown  in  figure  I4~2j)  with  the  underlying  idea  that  a  very  small  number  of  ICNAk 
(one  or  two)  with  a  very  accurate  estimate  of  their  positions  could  be  used  to  provide 
a  much  larger  group  of  Search,  Classify  and  Map  flSCMD-veliicles  with  navigation 
information.  These  ISCM1  vehicles  would  be  equipped  with  a  special  sonar  payload  to 
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Figure  4-2:  Hierarchical  cooperation  scenario  where  a  small  number  of  ICNAk  main¬ 
tains  a  very  accurate  position  estimate  while  a  much  larger  group  of  ISCM1  vehicles 
with  specialized  sonar  equipment  searches  the  sea  floor  for  mines.  Graphic  courtesy 
of  Bluefin  Robotics. 


detect  buried  or  free-floating  mines.  The  ICNAk  would  be  either  surface  crafts  with  a 
permanent  access  to  IGPSI  or  lAUVk  with  a  very  accurate  (and  expensive)  navigation 
suite.  To  maintain  a  bounded  uncertainty  on  their  position  estimates,  these  ICNAk 
would  move  at  a  very  shallow  depth  and  surface  for  a  IGPSI  fix  whenever  necessary. 
The  ISCMk  outfitted  with  much  simpler  (and  cheaper)  navigation  sensors  would  be 
able  to  maintain  a  bounded  uncertainty  on  their  position  estimates  without  surfacing 
over  the  entire  duration  of  the  mission. 

The  sole  mission  objective  of  the  ICNAk  is  to  minimize  the  overall  uncertainty  of 
the ISCMl  vehicles.  To  accomplish  this,  their  first  objective  is  to  maintain  a  very  good 
estimate  about  their  own  position,  as  in  Cooperative  Navigation  any  uncertainty  in 
the  ICNAI s  position  directly  translates  into  an  uncertainty  in  the  ISCMl s  position.  In 
addition,  the  relative  position  between  the  ICNAl  and  ISCMl  will  also  strongly  affect  the 
position  uncertainty  of  the  ISCMl  as  we  will  show  in  the  following  section.  Therefore 
the  second  objective  of  the  ICNAI  is  to  adjust  its  position  such  that  the  ICNAlfFTTVll 
geometry  is  optimal  for  ICNI 
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The  hierarchical  scenario  shown  in  figure  PfdZl  allows  us  to  position  ICNAk  as  dedicated 
navigation  beacons  for  our  IAIJV1  In  this  section  we  examine  the  effect  of  the  relative 
position  between  the  ICNAk  and  a  single  IAIJVI  We  ran  two  simulations  in  which 
the  relative  position  between  the  ICNAk  and  the  IAIJVI  was  the  only  parameter  we 
changed. 
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Table  4.1:  Variance  of  sensor  noise  for  the  simulated  vehicles  for  geometry  comparison 
(figure  14-31) 


Vehicle 

&U1&V 

erg 

(7  r 

Notes 

CNA  1 

0  m/s 

0° 

1  m 

ICNAk  have  iGPSl 

CNA  2 

0  m/s 

0° 

1  m 

AUV 

0.2  m/s 

o 

o 

1  m 

4.2.1  Simulation 

To  illustrate  the  effect  of  different  geometries  between  the  ICNAk  and  the  ISCMk  on 
the  position  accuracy  obtainable  through  ICNI  we  simulated  two  ICNI  missions  in  this 
section.  The  only  difference  between  the  missions  was  the  relative  position  between 
the  ICNAk  and  the  ISCmIR  For  both  simulations  we  assumed  the  ICNAk  to  be  surface 
crafts  with  access  to  IGPS1  We  simulate  the  ICNAk’  access  to  an  absolute  position 
estimate  by  setting  the  variance  of  the  measured  forward  au  and  starboard  speed  av 
as  well  as  the  heading  variance  ag  to  zero.  As  a  result  the  ICNAk  have  a  constant 
position  uncertainty  which  only  depends  on  the  quality  of  thelGPSlsignal.  The  I  AU  Vis 
sensor  variances  correspond  to  those  of  a  very  simple  navigation  suite  (table  I4~T1).  For 
both  simulated  runs  all  vehicles  traveled  over  a  distance  of  500  m  at  a  speed  of  1  m/s, 
while  maintaining  the  initial  geometry. 

4.2.2  Results 

Figure  EH  shows  the  results  for  both  simulated  runs.  Each  marker  “x”  shows  the 
position  of  all  vehicles  at  one  point  in  time  and  the  big  markers  “  X  ”  mark  an  arbitrary 
instant  to  illustrate  the  relative  position  between  the  vehicles.  As  all  vehicles  moved 
at  the  same  speed  and  on  the  same  course,  this  relative  position  was  maintained 
throughout  the  entire  mission.  Every  ten  seconds  one  of  the  two  ICNAk  broadcast 
its  position.  The  IATJVI  used  all  of  these  broadcasts  to  update  its  position  running 
an  IEKFI  (as  described  in  section  13.4.11)  and  a  IPFI  (as  described  in  section  13.4.21)  in 
parallel.  At  each  time  instant  marked  by  “x”  the  covariance  ellipse  as  well  as  the 
particle  set  at  that  time  instant  are  shown.  The  bottom  plots  for  both  runs  show 
how  the  uncertainty  evolves  by  representing  the  trace  of  the  covariance  matrix  P.  In 
the  case  of  the  Particle  Filter  (IPFI)  the  covariance  was  obtained  by  computing  the 
weighted  variance  for  the  entire  particle  set  as  described  in  equation  (13.161). 

The  two  top  plots  in  figure  14-31  show  the  results  for  a  run  with  a  very  disad¬ 
vantageous  relative  position  between  the  ICNAk  and  the  IAIJVI  throughout  the  entire 
mission.  As  a  result  of  the  near-collinear  configuration  the  updates  received  by  the 
IATJVI  only  “fixed”  the  position  in  the  east- west  direction,  while  the  uncertainty  in  the 
north-south  direction  continued  to  grow  without  bound,  as  illustrated  in  the  trace 

1  For  the  remainder  of  the  chapter  we  refer  to  the  ISCMI  simply  as  IAIJVI  to  illustrate  that  this 
approach  is  not  just  applicable  to  the  specialized  mine-hunting  scenario 
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plot  of  the  IEKFI  For  the  IPFI  the  mission  duration  is  too  short  to  see  if  the  uncer¬ 
tainty  of  the  position  estimate,  represented  by  the  spread  of  the  particle  set,  would 
have  kept  growing  unbounded  as  in  the  case  of  the  IFK.F1  The  two  bottom  plots  show 
a  run  in  which  the  lAUVl  was  consistently  at  the  tip  of  a  perpendicular  triangle.  This 
geometry  led  to  a  small  and  bounded  position  uncertainty  for  both  filters. 
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Figure  4-3:  Simulated  runs  with  twolCNAk  and  onelAUVl  Top  plots:  A  near-colinear 
(“bad”)  geometry  causes  the  lEKFls  uncertainty  to  grow  unbounded  and  the  IPFfs 
uncertainty  to  remain  high.  Bottom  plots:  A  “good”  geometry  keeps  the  lEKFls  and 
iPFls  uncertainty  bounded. 
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4.3  Dynamic  Positioning  for  Dedicated  Beacon  Ve¬ 
hicles 

In  the  previous  section,  we  showed  how  the  intra- vehicle  geometry  has  a  great  effect  on 
the  position  accuracy  obtainable  with  ICNl  In  a  hierarchical  scenario  with  dedicated 
ICNA1  vehicles  we  want  to  control  the  position  of  the  ICNAk  such  that  the  overall 
position  uncertainty  of  the  participating  lAUVk  is  minimized.  In  this  section  we 
present  a  decentralized  algorithm  which  runs  on  each  ICNAI  It  attempts  to  minimize 
the  overall  position  uncertainty  by  dynamically  computing  future  waypoints  based  on 
the  position  uncertainty  of  the  participating  lAUVk  and  the  predictable  trajectory  of 
other  ICNAk  We  first  introduce  different  metrics  which  can  be  used  to  quantify  the 
position  uncertainty  and  then  explain  the  assumptions  made  for  our  algorithm.  The 
remaining  sections  then  present  our  algorithm  and  simulation  results. 

4.3.1  Motivation 

Figure  lT3l  illustrates  how,  by  just  choosing  the  relative  position  between  ICNAk  and 
I  AT I  Vk  one  can  bound  the  lAIJVk  position  uncertainty  allowing  the  I  AT  J  Vk  to  remain 
submerged  during  long  missions,  or  how  the  ICNAl  can  have  very  little  positive  effect, 
requiring  the  lAUVk  to  surface  for  a  position  fix  to  enforce  a  bounded  uncertainty. 
This  motivated  the  active  positioning  of  the  ICNAk 

The  special  case  of  a  hierarchical  ICNl  scenario  in  which  vehicles  use  consecutive 
range/position  pairs  from  moving  beacons  combined  with  dead- reckoning  information, 
has  not  been  the  subject  of  substantial  research.  This  reflects  the  reality  that  robots 
outside  the  underwater  realm  can  usually  obtain  range  or  bearing  measurements 
to  several  known  landmarks  at  one  time  and  combine  them  to  a  position  estimate. 
Results  from  the  large  body  of  research  carried  out  for  static  trilateratiorU  to  (certain 
or  uncertain)  landmarks  can  provide  insights  into  the  problem  of  motion  planning 
for  dedicated  beacon  vehicles  (ICNAk).  As  range  measurements  are  usually  readily 
available  underwater  we  will  focus  on  trilateration. 

4.3.2  Related  Work 

In  particular,  IGPSI  navigation  relies  on  trilateration  to  satellites  at  known  positions, 
and  hence  there  is  a  large  body  of  work  that  addresses  the  uncertainty  of  trilat¬ 
eration  fixes.  The  metric  most  frequently  employed  is  the  geometric  dilution  of 
precision  (IGDOPI) .  A  single  dimensionless  number  which  captures  the  influence  of  the 
geometry  on  the  error  of  the  position  estimate.  Examples  of  work  that  investigates 
error  metrics  for  GPS  trilateration  includes  Chaffee  and  Abel  [2QJ  and  McKay  and 
Pachter  [5i£|.  This  literature  typically  assumes  precise  knowledge  of  the  positions  of 
satellites,  and  hence  the  impact  of  the  beacon  position  error  is  not  analyzed.  Early 
work  in  multi-robot  localization,  which  used  the  concept  of  “portable  landmarks”,  in 

Estimating  a  position  is  called  trilateration  if  range  measurements  to  landmarks  at  known 
positions  are  used  and  triangulation  if  the  position  is  obtained  by  using  angle  measurements. 
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which  a  stationary  group  of  robots  would  serve  as  landmarks  to  a  moving  group  of 
landmarks,  did  not  take  the  trilateration  and  triangulation-error  into  account  [54]. 
Later,  more  dynamic  scenarios  were  investigated  in  which  all  robots  tried  to  improve 
their  position  estimates  through  range  or  angle  measurements  to  other  robots  [74] . 
Here  the  uncertainty  estimate  of  the  landmarks  (other  robots)  was  used  implicitly 
during  the  position  estimate,  but  no  attempt  was  made  to  explicitly  analyze  its  mag¬ 
nitude  and  its  dependence  on  the  geometry  In  feature-based  Simultaneous  Local¬ 
ization  and  Mapping  lISI.AMIl.  the  environment  is  typically  represented  in  terms  of 
discrete  landmarks,  and  hence  in  such  a  formulation  uncertainty  in  the  landmark  lo¬ 
cation  is  incorporated  explicitly.  Feature-based  ISL  AMI  from  range-only  measurements 
has  been  addressed  by  several  authors,  including  Djugash  et  al.  [25],  who  developed 
ISL  AMI  algorithms  for  range-only  measurements  using  custom  sensor  nodes,  and  Wijk 
and  Christensen  [HI] ,  who  performed  ISL  A  Ml  via  trilateration  using  range  measure¬ 
ments  from  ultrasonic  sensors.  In  a  ISLAMI  formulation,  the  position  estimate  for  the 
vehicle  implicitly  accounts  for  the  (correlated)  uncertainty  in  the  position  estimates 
of  the  landmarks.  As  the  position  of  the  landmarks  in  the  range-onlv  ISLAMl  research 
cannot  be  controlled  the  topic  of  actively  positioning  the  beacons  to  minimize  the 
uncertainty  has  not  yet  been  addressed. 

Trilateration  is  a  central  component  of  methods  to  calibrate  the  positions  of  net¬ 
works  of  sensors  with  range-only  measurement  capabilities.  Moore  m  presents  an 
algorithm  which  selects  landmarks  for  trilateration  such  that  the  resultant  geometry 
ensures  a  unique  solution  in  the  presence  of  measurement  noise. 

Research  that  specifically  investigates  the  error  of  trilateration-  (or  triangulation) 
based  localization  techniques  is  fairly  sparse.  Kelly  [52]  provides  a  useful  and  intuitive 
insight  into  the  relationship  between  robot/landmark-geometry  and  the  resulting  po¬ 
sition  accuracy,  but  he  does  not  take  the  uncertainty  of  the  landmarks  into  account. 
Easton  and  Cameron  [27]  explicitly  assume  noisy  landmarks  for  triangulation-based 
algorithms,  but  their  method  does  not  consider  the  effects  of  strongly  skewed  error 
distributions  (as  shown  in  figure  14-411  and  their  effect  on  the  optimal  triangulation 
geometry.  As  a  result,  the  uncertainty  of  the  triangulated  position  chosen  by  their 
algorithm  is  affected  by  the  sensor  noise,  but  the  point  at  which  triangulation  leads 
to  the  smallest  possible  error  for  given  landmark  uncertainties  does  not  vary. 

The  special  case  where  beacon  vehicles  are  actively  positioned  to  improve  self¬ 
localization  of  other  vehicles  has  not  been  the  subject  of  much  previous  work.  A 
similar  case  however  is  investigated  by  Trawny  [86] .  Using  a  single  monolithic  filter 
he  chooses  a  path  which  minimizes  the  joint  covariance  matrix  for  all  vehicles.  In 
addition  there  is  a  large  body  of  work  where  a  single  robot  or  a  group  of  robots  is 
positioning  itself  to  track  a  target  (Zhou  and  Roumeliotis  [96] )  explore  an  environ¬ 
ment  (Feder  and  Leonard  [35] )  or  determine  the  location  of  a  source  (Christopoulos 
and  Roumeliotis  [21]).  In  the  underwater  domain  Paley  [68]  presents  several  dis¬ 
tributed  control  mechanisms  for  efficient  ocean  sampling.  Results  from  this  research 
can  provide  insights  into  the  beacon  positioning  problem. 
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4.3.3  Metrics  for  Position  Uncertainty 

Three  metrics  are  widely  used  to  describe  the  error  ellipse  of  a  bivariate  Gaussian 
distribution  with  a  single  parameter  [27]. 

Area  of  the  Covariance  Ellipse  The  area  A  of  the  3cr-error  ellipse  which  is  related 
to  the  covariance  matrix  P  by  A  =  7r  *  \J det(P). 


Ratio  of  the  Semi-minor  to  the  Semi-major  Axis  The  ratio  of  the  semi-minor 
to  the  semi-major  axis  of  the  error  ellipse  p  =  (cq  <  <r2) 

Trace  of  the  Covariance  Matrix  trace(P). 


As  these  three  metrics  assume  that  the  distribution  of  the  position  estimate 
is  Gaussian,  we  propose  as  an  additional  metric  a  variation  of  the  Circular  Error 
Probable  (IGEPl).  The  majority  of  published  research  defines  the  IGEPl  as  the  proba¬ 
bility,  p(R),  that  a  single  realization  of  a  mean- free,  bivariate  Gaussian  distribution 
N(0,P)  is  within  a  circle  with  radius  R  [32].  A  variation  which  defines  the  IGEPl 
as  the  radius  R(p)  of  the  circle  which  contains  half  the  realizations  (for  p  =  0.5)  of 
N( 0,  P )  is  described  by  Torrieri 


JJp{x)dx  =  0.5  R=  {\x-E[x]\  <  CEP} 


(4.1) 


R 

While  most  applications  which  use  the  IGEPl  assume  a  bivariate  Gaussian  for  the 
distribution  T  in  equation  (j4.ll)  the  key  advantage  of  this  metric  is  that  it  can  be 
used  for  any  2D  probability  distribution. 

There  is  no  closed  form  solution  for  p(R)  or  R(p),  but  Shnidman  provides  an  effi¬ 
cient  algorithm  to  compute  R(p)  in  case  T  is  a  bivariate  Gaussian  [78].  Shnidman’s 
algorithm  can  also  be  generalized  for  p  ^  0.5,  but  R(p  =  0.5)  provides  a  parameter 
with  an  intuitive  understanding  of  an  “average  error” .  For  p  <C  1  Shnidman’s  algo¬ 
rithm  can  run  into  underflow  problems,  but  as  in  this  case  the  bivariate  approaches 
a  one-dimensional  Gaussian  with  variance  cq,  we  can  compute  R  using  the  inverse 
error  function 


R  =  V2  *  cr2  *  erf  1(P) 


(4.2) 


A  function  to  evaluate  the  inverse  error  function  is  provided  in  MATLAB  and  is  also 
described  in  [95]. 


4.3.4  Trilateration  from  Uncertain  Static  Beacons 


To  gain  insight  into  the  effect  of  geometry  between  ICNAk  and  lAUVk  on  the  uncer¬ 
tainty  of  the  lAUVk ?  position  estimate  we  examine  the  case  in  which  two  ICNAk  are 
held  in  a  fixed  position  and  an  IAUVI  can  position  itself  freely  in  the  2D  plane  (fig¬ 
ure  03D.  The  lAUV^theu  localizes  itself  in  the  plane  using  two  range  measurements 
(one  to  each  IGNAIrl.  Both  IGNAk  have  uncertainty  associated  with  their  positions. 


3We  assume  that  the  ambiguity  has  been  resolved  and  the  correct  solution  of  the  two  has  been 
selected. 
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Figure  4-4:  Optimal  trilateration  positions  for  different  error  distributions,  green, 
solid:  <7i  =  (72  and  P\  =  P 2.  red,  dashed:  cq  <C  <72  and  P\  7^  P2 


We  then  compute  the  error  associated  with  the  I  ATI  Vis  position  estimate.  The  metric 
used  is  the  ICEPI  as  described  in  the  previous  section.  We  then  determine  the  optimal 
trilateration  point  xopt,  i.e.  the  point  with  the  smallest  ICEPI  In  the  special  case  in 
which  <7 1  =  (72  for  P 1  and  P 2  the  optimal  position  of  the  robot  is  the  corner  of  a  right- 
angled  triangle,  with  the  landmarks  in  the  two  other  corners  (green  in  figure  ITT)  as 
shown  by  However  in  all  other  cases,  in  which  cq  ^  <j2  for  Pi  or  P2  the  optimal 
position  strongly  depends  on  the  shape  and  orientation  of  the  error  ellipses  (red  in 
figure  14-41) .  We  show  in  [7]  that  there  is  no  closed  form  solution  to  obtain  xopt  even 
for  the  simple  scenario  shown  in  figure  14-41 

4.3.5  Assumptions 

Our  algorithm  computes  the  optimal  future  position  of  a  ICNAI  such  that  position  in¬ 
formation  broadcast  from  this  position  by  the  ICNAl  will  reduce  the  combined  position 
uncertainty  of  all  lAUVT  by  the  largest  amount.  The  algorithm  is  decentralized  and 
as  such  only  incorporates  information  which  is  locally  available  or  overheard  through 
the  acoustic  channel.  Using  decentralized  algorithms  is  a  key  requirement  in  the 
underwater  domain  as  the  reliable  communication  channel  to  a  single  controller,  as 
required  by  centralized  topologies,  is  not  available. 

The  metric  which  is  minimized  in  this  version  of  the  algorithm  is  the  sum  of  the 
trace  differences  between  the  prior  and  posterior  covariances  of  the  IAIJW  position 
estimates.  This  metric  assumes  that  the  navigation  algorithm  running  on  all  vehicles 
is  an  IEKFI  as  described  in  section  13.4.11  The  algorithm  however  can  accommodate 
other  Bayes  filters  and  any  state  representation  by  modifying  line  El  in  algorithm  Eland 
line  El  in  algorithm  |H]  accordingly.  Also,  the  metric  which  is  minimized  can  be  changed 
to  other  metrics  such  as  those  proposed  in  14.3.31  by  modifying  line  [5]  in  algorithm  El 

The  following  assumptions  are  made  by  the  adaptive  positioning  algorithm: 
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Figure  4-5:  ICEPI  and  covariance  ellipses  for  trilateration  solutions.  Left:  the  ICEPI 
for  positions  obtained  through  trilateration  from  the  two  static  and  uncertain  ICNAI 
positions.  Right:  covariance  ellipses  for  selected  trilateration  solutions  for  the  same 
scenario.  In  both  figures  the  point  with  the  smallest  ICEPI  is  marked  with  a  magenta 

V. 


Vehicles 

There  are  two  groups  of  vehicles.  A  group  of  lAUVk  A,  which  carry  out  a  mission 
and  a  group  of  ICNAk.  C,  which  serve  as  moving  navigation  beacons.  Optimizing 
the  relative  position  between  ICNAI  and  a  lAUVk  is  entirely  left  to  the  ICNAk  as  it  is 
assumed  that  each lAUVfs  track  is  solely  controlled  by  its  mission  objective.  NoICNAl 
needs  to  be  aware  a  priori  of  all  members  of  the  set  of  participating  IAT  J  Vk  and  ICNAk 
The  sets  A  and  C  can  be  updated  dynamically. 

Communication 

Each  member  of  A  and  C  shall  be  outfitted  with  an  acoustic  modem  for  data  trans¬ 
mission  and  intra-vehicle  ranging.  As  only  one  vehicle  can  transmit  at  any  given  time, 
there  will  be  a  schedule  S  which  assigns  a  time  slot  during  which  a  vehicle  (ICNAI  or 
IAIJVI)  can  broadcast  a  status  message.  The  schedule  S  is,  either,  provided  to  all  ve¬ 
hicles  before  the  mission  starts  or,  in  the  case  of  a  central  communications  controller 
which  initializes  communication  through  polling,  the  vehicles  “learn”  the  schedule 
as  they  overhear  polling  requests.  It  is  assumed  that  the  schedule  is  repetitive  and 
does  not  change  over  a  longer  period  of  time  such  that  predictions  about  the  time 
of  future  transmissions  are  possible  once  S  is  known.  Each  entry  in  S  consists  of  a 
vehicle  identification  number,  i,  and  a  broadcast  time,  which  is  relative  to  the  start 
of  the  schedule.  When  a  vehicle  i  broadcasts,  its  transmission  m,  not  only  contains 
the  actual  distribution  over  its  pose  estimate  xt,  but  also  its  course  0-,  and  speed  vt  or 
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even  a  short  description  of  the  upcoming  mission  plan.  This  will  enable  every  other 
vehicle  overhearing  this  message  to  compute  a  short-time  prediction  of  the  vehicle’s 
future  position.  The  message  also  contains  a  unique  vehicle  identification  number  i. 
Each  vehicle  also  stores  the  predicted  positions  ofICNAk  and  lAIJVk  in  the  according 
entries  in  A  or  C.  While  section  ld~fl  outlines  several  ways  to  represent  the  distribution 
of  the  state  estimate,  we  chose  to  parametrize  it  through  a  mean  and  a  covariance 
Pi  as  this  is  the  most  compact  representation  and  therefore  most  suitable  one  for 
acoustic  communication. 

Sensors 

Optionally,  the  ICNAk  may  have  available  to  them  a  sensor  table  J\f  which  contains 
a  set  of  tuples,  in  which  each  tuple  n*  E  JV  contains  information  about  the  i-th 
sensor’s  capabilities.  If  this  information  is  available  to  the  ICNAl  it  can  also  carry  out 
short-term  predictions  about  the  future  position  and  uncertainties  of  the  lAIJVk  and 

pro. 

4.3.6  Algorithm 

The  adaptive  positioning  algorithm  consists  of  four  modules  (Algorithm  El  El  El  and 
H,  which  are  run  on  each  ICNAl  individually  when  the  appropriate  conditions  are  met. 
Algorithm  El  and  [HI  both  call  the  function  algorithm  El  which  computes  the  optimal 
ICNAl  position  for  a  given  setup  of  ICNAk  and  lAIJVk 

Algorithm  [6]  is  run  whenever  the  ICNAl  receives  a  broadcast  from  an  I  All  VI 

Algorithm  [7]  is  run  whenever  the  ICNAl  receives  a  broadcast  from  another  ICNAl 

Algorithm  [8]  is  run  whenever  the  schedule  S  indicates  that  the  ICNAl  should  broad¬ 
cast. 

Algorithm  [9]  is  a  function  which  computes  an  optimal  future  ICNAl  position  when 
the  position  and  associated  uncertainties  of  all  ICNAk  and  lAIJVk  have  been 
predicted  for  this  time. 

Message  Reception  from  an  IAUVI  (Algorithm  ED 

When  a  ICNAl  receives  a  broadcast  from  an  IAUVI  ,  it  decodes  the  message  (line  ED 
and  uses  it  to  update  its  estimate  of  the  future  positions  and  associated  uncertainties 
of  cij  up  to  the  next  time  t\  (line  ED  at  which  the  ICNAl  is  scheduled  to  broadcast.  It 
achieves  this  by  forward  projection  using  Oj’s  actual  position  course  and  speed  (line ED 
and  the  information  about  a/s  sensor  quality  which  is  retrieved  from  J\f,  (j ) .  If  the 
received  message  mf(t0)  from  a3  contains  a  description  of  its  short  term  mission  plan 
an  even  more  accurate  prediction  can  be  made.  For  the  scenario  we  use  to  illustrate 
the  algorithm,  all  predictions  are  based  on  available  course  and  speed  information. 
The  functions  g(-)  and  h(-)  in  line  El  also  use  the  information  locally  stored  in  Ct  so 
as  to  consider  the  message  broadcasts  from  all  other  ICNAk  which  occur  between  the 
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current  time  (to)  and  t\  and  how  they  will  affect  the  lAIJVV  position  estimate  at  the 
time  t1-.  The  updated  information  about  a,  is  stored  in  At(j.  t\)  (line  EJ) - 


Require:  Ai,CijSi,J\fi 
1:  loop 

2:  if  message  mf  received  from  AUV  a3  E  A,  then 


mf(t0)  = 


xf 

PA 

vj 

ef 

j 


tbi  =  f(to,Si(i)) 

„A(+b\  _ 


xfW)  =  g(xf(t0),vf(t0),ef(t0)Ai,Ci) 

Pf(tbi )  =  h(xf(t0),Pf(t0),vf(t0),8f(t0),tb,W),Ci) 
tl  xf(1%),  Pf(tbi)  ->  Ai(j,  t\) 

end  if 
end  loop 


Algorithm  6:  Executed  on  ICNAI  whenever  a  message  from  an  IAUVI  is  received. 


Message  Reception  from  Another  ICNAI  (Algorithm  [3) 

When  a  message  is  received  from  ICNAI  c3  it  shall  contain  a  more  recent  estimate  of 
the  ICNAfs  state  estimate  xA ,  the  associated  uncertainty  Pc-  as  well  as  the  actual 
course  and  speed  (estimates)  v A  and  6^  (line  ED-  The  algorithm  then  locally  emulates 
the  effect  that  that  specific  broadcast  would  have  had  on  the  positioning  estimate  of 
all  IAUVI;  assuming  that  alllAUVb  received  the  message.  This  is  carried  out  as  follows: 
Firstly,  it  fetches  the  predicted  position,  x£,  and  uncertainty  estimate,  Pk ,  for  the 
actual  time  to  for  each  lATJVl  in  Ai  from  its  IAUVI  table  (line  [5]).  It  then  updates  the 
position  and  uncertainty  of  each  IAUVI  using  the  Kalman  state  update  (13.131)  and  the 
uncertainties  using  the  Kalman  covariance  update  (13.141)  (line  IHJ)  and  then  stores  the 
the  resultant  estimate  back  into  the  table  Ai(k )  (lineEj). 

Algorithm  0  then  duplicates  the  decision  making  process  taking  place  at  ICNAI  c,-. 
Using  the  communications  schedule  it  computes  the  point  in  time,  t1:’,  at  which 

ICNAI Cj  will  broadcast  again  (line El).  Calling  the  function  compute_opt_CN Apposition 
(algorithm  [9])  with  the  actual  position  of  Cj  obtained  from  m^(to)  and  our  local 
knowledge  of  the  future  positions  of  the  I  AT  J  AT  and  the  ICNAk  we  can  compute  the 
optimal  position  £Cy''opt )  for  Cj  I  lineim).  If  all  information  transmitted  through  the 
acoustic  modems  was  received  by  all  vehicles,  then  ICNAI;  ct  and  c3  will  have  the  same 
positioning  information  available  and  computed  locally  by  c3,  should  be 

the  same  location  computed  by  cx .  If  not  all  values  were  equally  shared,  e(  and  Cj 
will  compute  different  values,  but  in  the  absence  of  any  other  information  cc^opt(^) 
is  the  best  prediction  for  c/s  position  at  t1’.  Additionally  we  use  the  table  entry 
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Require:  Ai,Ci,Si,Ni 
1:  loop 

2:  if  message  m£  received  from  CNA  Cj  E  Ci  then 


4: 

5: 


9: 

10: 

11: 

12: 

13: 

14: 


3:  mf(t0)  = 


X?  1 

PI 

v y 

3 


for  all  0^  e  Ai  do 

A  (Mo)  ->x£(t0),p£(t0) 


x£(to) 
(to) 


13131. xf.Pf 


Xk(t  o) 

-PjfC^b 


x£(t0 ),p£(t0)  ->  a  (Mo) 

end  for 

=  f(t0,Si(j )) 


x£opt(tbj)  <—  compute.opLCNA.position(tbj,Xj!(t0),Ai(tbj),Ci(tbj))  (Alg.  | 


pC(tb)  =  h(xC(t0),pC(t0),x<. 

tb 

Lj 

end  if 


*$>  ®?_opt(*$)>  -Pf  (tj)  -»•  Ci(j, 


V),V(j)) 


end  loop 


Algorithm  7:  Executed  on  a  ICNAI  whenever  a  message  from  another  ICNAI  is  received. 


for  Cj’s  sensor  noise  characteristics  A/”t(j)  to  predict  the  future  position  uncertainty 
at  xf'opt(tb)  (line  fTTi).  The  new  estimate  about  c/s  future  positions  is  updated  in 

Ci(j,tbj)  (lme[ni). 


ICNAI  broadcast  (Algorithm  [8]) 

When  the  actual  time,  to,  matches  its  scheduled  broadcast  time,  tb ,  ICNAI  c,  hrst 
broadcasts  a  message  mf(t0)  containing  its  actual  position  estimate  xff  associated 
covariance  Pf  as  well  as  its  actual  course  Of'  and  speed  vf  (line  |3])  in  a  similar 
manner  to  that  of  algorithm  [71  First,  the  effect  that  this  ICNAfs  position  broadcast 
would  have  on  each  IATJVI  is  modeled,  in  which  it  is  assumed  that  each  received  the 
latest  broadcast  mf(to)  (line  [5],  [6]  and  [7)).  Then  using  the  schedule  St  the  next 
broadcast  time  tb  is  computed  (line  [H]) .  At  this  time  all  available  information  about 
the  positions  of  each  ICNAI  and  lATTVl  at  tb  (from  Ai(tb )  and  Ci(tb ))  is  used  to  determine 
the  optimal  position,  xf'opt(tf)  at  which  the  ICNAfs  next  broadcast  should  take  place 
(line  [TOj) .  The  position  uncertainty  accumulated  up  to  xfopt(tb)  is  predicted  based 
on  the  actual  position  and  uncertainty,  as  well  as  the  future  position  and  the  sensor 
noise  Mt  lline  fill) .  All  updated  information  is  stored  in  Ct  (i ,  tb )  time  fITll . 
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Require:  Ai,Ci,Si,Ni 
1:  loop 

2:  if  =  t\  then 


3:  broadcast  mf  (t0)  = 


4: 

5: 


9 

10 

11 

12 

13 

14 


xf 

p? 

V. ? 

e? 


—.4  , 


for  all  0^  e  Ai  do 

A  (Mo)  -*•  x£(t0),Pk(t0) 

SjHto)  x£(to) 

PtM  Pi  (to) 

*fc(^0 ),Pk(to)  -»•  A  (Mo) 

end  for 

tbi  =  f(t0,Si) 

xfoptiA)  compute-opt-CNA-position(tl!,xf(t0),Ai(tbi),Ci(tbi))  {Alg. E3} 

Pf(tb)  =  h(xf(t0),Pf(t0),xf_opt^),Afi) 

tb,  xi-opt(tbi),  PCi  Ci)  -*■  Mb  *$) 

end  if 
end  loop 


Algorithm  8:  Executed  on  a  ICN  AI  whenever  it  is  scheduled  to  broadcast. 


Determining  the  Optimal  ICNAI  Position  (Algorithm  [9]) 

This  function  computes  the  optimal  ICNAI  position  for  a  desired  time,  tb,  assuming 
that  the  predicted  position  of  all  other  ICNAk  in  Cr  and  the  positions  for  all  lAUVb  in 
Ai  are  available. 

As  we  showed  in  14.3.41  that  there  is  no  closed  form  solution  to  find  the  optimal 
beacon  point,  we  chose  a  brute-force  approach.  The  function  first  computes  a  grid  of 
discrete  positions  M  which  could  possibly  be  reached  by  the  ICNAI  before  the  next 
broadcast  (lined]).  The  number  of  grid  positions  in  M  depends  on  the  maximum 
speed  of  the  vehicle,  vrnax ,  the  time  between  now  (to)  and  the  next  broadcast  t\  and 
the  spacing  of  the  grid  points.  As  the  runtime  of  the  function  is  linearly  dependent 
on  the  number  of  grid  points,  the  grid  spacing  can  be  varied  depending  on  vmax ,  tb 
and  the  available  CPU  cycles. 

For  each  grid  point,  Xp  in  AT,  we  now  compute  by  how  much  the  overall  position 
uncertainty  would  be  reduced  if  it  would  broadcast  from  this  point  at  tb.  It  does  this 
by  fetching  the  position  x^(tb)  for  each  I  All  VI  at,  (lined])  and  computing  the  differ¬ 
ence  between  the  trace  of  the  prior  Pk  (tb)  and  posterior  covariance  matrix  Pk(tb), 
assuming  a  Kalman  update  (18.141)  by  ct  from  position  x^.  The  trace  differences  for 
all  I  At  J  AT  are  summed  up  and  stored  in  K  (line  [5]).  K  has  the  same  size  as  AT.  After 
the  total  achievable  improvement  has  been  computed  for  all  x%(t% ),  we  determine  the 
largest  entry  in  K.  The  position  which  maps  to  this  entry  is  the  optimal  position 
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Require: 


1: 

2: 

3: 

4: 


5  Xp  5 


M  —  t 

for  all  x^eMdo 
for  all  ctk  E  A%  do 


’  X q  J 


V  Xp  s.t. 


I  rpC  _  rpC  I 
^p  | 


2  <  Vimax(tbi  ~  t0) 


Pitt) 


K(p)  =  T,  trace  (P^(^)-P^(^) 


yAf'b) 


end  for 
end  for 

_  _  max  (K")  ^  ,1V 

M  -i  '*£*,«) 

.c  ufel 

tv6*; 


—A  <3A4l.a!k-. 

Pt($) 


**£(*!) 


9:  return  a; 


P-Opt  \ 


Algorithm  9:  Compute  the  optimal  position  xc;jpt  for  a  ICNAI  c%  for  a  predicted  time 
.  It  assumes  that  the  position  and  uncertainties  for  all  other  vehicles  (ICNAk  and 
lAUVh)  are  given  by  At  and  C% . 


Table  4.2:  Sensor  noise  and  maximum  speed  of  the  simulated  vehicles  used  in  the 
adaptive  positioning  simulation  (figure  14-61  and  14-71) . 


Vehicle 

G  r 

Vmax 

Notes 

CNA  1 

0  m/s 

0° 

2m 

1.5  m/s 

ICNAk  haveIGPSI 

CNA  2 

0  m/s 

0° 

2m 

1.5  m/s 

not  in  scenario  1 

AUV  1 

0.2  m/s 

1— * 
o 

o 

1  m 

1  m/s 

AUV  2 

0.2  m/s 

10° 

1  m 

1  m/s 

not  in  scenario  1 

opt  t°  which  the  ICNAI  should  move  so  as  to  maximally  reduce  the  uncertainty  of 
the  IAIJV1  set  (line  IHJ) - 


4.3.7  Results 

To  test  this  adaptive  positioning  algorithm  we  simulat  two  scenarios.  The  first  sce¬ 
nario  (figure  14-61)  consists  of  one  IAUVI  and  one  ICNAI  in  which  both  vehicles  start  at 
the  same  point  and  the  IAUVI  mission  takes  it  on  a  straight  west-east  trajectory  for 
400  m.  The  second  scenario  (figure  14-71)  uses  two  IAUVT  and  two  ICNAk.  All  vehicles 
start  at  the  same  point  with  IAUVI  1  moving  north  for  100  m  and  IAUVI  2  moving 
south  for  100  m.  Both  lAt/Vls  then  move  on  a  west-east  trajectory  while  maintaining 
their  200  m  separation.  The  simulated  sensor  noise  is  equivalent  to  an  IAUVI  with  an 
inexpensive  navigation  suite.  The  variances  of  the  sensor  noise  for  both  simulations 
are  the  same  as  those  used  in  section  14.1.21  and  are  shown  in  table  PI 
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One  IAIIVL  One  ICNAl 


Figure 033 shows  the  simulation  results  for  the  most  basic  possible ICNI setup.  oneICNAl 
and  one  I  ATI  VI  Every  60  seconds  the  ICNAl  broadcasts  its  position  and  then  computes 
the  optimal  position  for  the  next  broadcast.  As  there  are  no  other  ICNAk  present, 
the  ICNAl  only  needs  to  take  the  effect  of  its  own  updates  and  the  vehicles’  sensor 
performance  into  account.  The  top  plot,  at  t=20  s,  shows  the  situation  directly  after 
the  mission  commenced.  The  ICNAl  has  just  broadcast  its  position  and  the  position 
it  predicts  for  the  I  AT!  VI  at  the  next  broadcast  which  is  marked  with  red  “+” .  The 
semi-transparent  circle  with  radius  r  =  At  ■  vmax  =  60s  •  2 m/s  =  120  m  marks  all 
positions  which  the  ICNAl  could  reach  at  maximum  speed.  Our  algorithm  discretizes 
this  circle  into  grid  points  with  5  m  spacing.  It  then  computes,  for  each  grid  point, 
the  position  uncertainty  which  the  IAUVI  would  have  after  a  hypothetical  update 
broadcast  by  the  ICNAl  from  this  grid  position.  The  difference  between  the  prior 
and  posterior  trace  of  the  lAUVV  position  estimate  is  represented  by  the  color  of  the 
semi-transparent  circle.  Positions  marked  blue  would  lead  to  a  very  small  decrease 
in  overall  uncertainty  and  positions  marked  red  would  lead  to  a  very  high  overall 
decrease.  The  mapping  between  the  absolute  value  of  K(p)  and  the  color  is  scaled, 
each  time  the  circle  is  plotted,  to  span  the  maximum  color  space.  Thus  we  cannot 
provide  a  legend  which  maps  colors  to  absolute  values  for  K  (p) .  The  position  which 
corresponds  to  the  maximum  of  that  difference  is  selected  as  the  future  position  for 

he  EMI 

As  the  IAUVI  has  a  high  variance  in  its  heading  direction  it  accumulates  the  highest 
uncertainty  in  the  direction  perpendicular  to  the  direction  it  is  traveling  in.  As  shown 
by  Zhou  and  Roumeliotis  in  [96],  the  biggest  decrease  in  the  trace  of  the  covariance 
can  be  achieved  if  the  beacon  vehicle  is  somewhere  along  the  semi-major  axis  of  the 
IAUVI s  covariance  ellipse.  Brute-force  computation  confirms  this,  by  highly  favoring 
positions  perpendicular  to  the  direction  in  which  the  IAUVI  is  traveling,  illustrated 
in  dark  red,  for  the  first  update.  At  t=72  s  (middle  plot)  the  ICNAl  has  reached  its 
planned  position.  The  IAUVI  has  reached  its  predicted  position  and  the  ICNAl  has 
transmitted  its  message  and  computed  a  new  optimal  broadcast  position  for  its  new 
message.  As  the  previous  broadcast,  at  t— 70  s,  strongly  reduced  the  error  in  the 
north-south  direction,  the  along-track  error  will  dominate  the  position  uncertainty 
and  the  optimal  position  is  in  fine  with  the  vehicle  traveling.  The  bottom  plot,  at 
t=320  s,  shows  the  vehicles  after  the  fifth  broadcast.  At  this  stage  a  “saw-tooth” 
pattern  has  been  established,  in  which  the  ICNAl  oscillates  between  the  two  relative 
positions  which  can  be  seen  in  the  top  and  middle  plot. 

Due  to  the  much  larger  distances  that  the  ICNAl  has  to  travel  in  this  scenario, 
compared  to  those  of  the  IAUVI  the  distance  between  the  ICNAl  and  the  IAUVI  slowly 
increases,  as  reaching  the  optimal  relative  position  is  theICNATs  only  goal.  Section  ITU 
addresses  how  the  course  and  speed  of  the  ICNAl  can  be  determined  if  reaching  the 
optimal  position  is  only  one  of  the  ICNAl s  objectives.  Another  objective,  for  example, 
would  enforce  a  minimum  distance  between  the  vehicles. 


106 


Chapter  4.  Intra- Vehicle  Geometries  for  Cooperating  I  AT  JVk 


Two  lAITVh.  Two  ICNAk 

A  more  complex  ICNlscen.ar.io  is  shown  in  figure  14-71  Here,  two  ICNAk  try  to  jointly 
optimize  their  trajectory  to  improve  the  position  uncertainty  for  two  lAIJVk  All 
four  vehicles  start  at  the  same  position  and  both  ICNAk  broadcast  their  position 
every  30  s.  After  [CNAl  1  broadcasts  its  first  message,  at  t=10  s,  it  determines  that 
the  position  marked  by  the  blue  “+”  is  the  optimal  position  for  its  next  broadcast. 
Meanwhile  ICNAI 2  waits  until  its  first  broadcast,  at  t=40  s,  and  then  determines  its 
optimal  position  for  its  next  broadcast  at  t=100  s  (cyan  When  computing 

the  trace  difference  represented  by  the  semi-transparent  circle  in  the  middle  plot  (the 
corresponding  circle  for  ICNAI  1  is  not  shown  as  they  would  overlap) ,  ICNAI  2  takes  the 
effects  of  the  broadcast  from  ICNAI  1  at  t=70  s  into  account,  as  otherwise  it  would  also 
head  for  the  optimal  position  previously  computed  bv ICNAI! .  leading  to  a  redundant 
update.  Shortly  after  ICNAI  2  reaches  its  computed  position,  all  four  vehicles  achieve 
the  stable  position  of  a  quadrilateral  which  is  maintained  throughout  the  mission 
and  can  be  seen  in  the  bottom  plot.  The  bottom  plot  also  shows  the  two  predicted 
positions  for  both  lAUVk 


4.4  Optimal  Positioning  in  a  Multi- Objective  De¬ 
cision  Scenario 

The  “one  IAIJVI  one  ICNAI  scenario”  depicted  in  figure  14-61  shows  how  optimizing 
the  trajectory  for  the  short-term  optimal  broadcast  position  alone  can  lead  to  a  sub- 
optimal  long-term  solution  as  the  distance  between  the  vehicles  constantly  grows  until 
the  distance  is  too  long  for  transmission.  Therefore  we  would  also  like  the  dynamic 
positioning  of  our  ICNAk  to  be  influenced  by  other  objectives  such  as  maintaining  a 
minimum  distance  to  all  vehicles.  If  the  acoustic  propagation  conditions  are  known, 
choosing  the  broadcast  position  such  that  the  transmission  loss  to  all  vehicles  is 
minimized  could  be  another  possible  objective. 

The  problem  of  selecting  an  action  for  an  agent,  in  our  case  the  speed  and  course 
of  our  ICNAl  in  a  situation  in  which  several  objectives  have  to  be  satisfied  has  been  the 
subject  of  extensive  research  i,  0.  These  methods  typically  switch  between  satis¬ 
fying  the  different  goals  individually  or  perform  averaging  which  does  not  necessarily 
lead  to  the  optimal  solution  as  illustrated  in  m 

More  recently  Benjamin  developed  the  Interval  Programming  flIVPI)-met,hod  which 
can  compute  an  optimal  solution  for  a  set  of  piece- wise  linear  objective  functions  m 
EE3J.  This  method  was  implemented  in  the  Mission  Oriented  Operating  Suite  flMOOSD- 
environment,  a  software  suite  developed  by  Newman  [61J  and  used  bvIMITIto  control 
various  land,  surface  and  underwater  robots.  This  implementation  was  tested  in  sev¬ 
eral  different  scenarios  and  has  demonstrated  an  I  ASCI  successfully  reaching  a  waypoint 
while  observing  the  “rules  of  the  road”  m  and  tracking  underwater  targets  with  a 
towed  array  while  ensuring  that  its  maneuvers  would  not  damage  the  array  pj. 

The  output  of  our  adaptive  positioning  algorithm  can  be  directly  expressed  as  an 
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objective  function  which  the  IIVPI  method  controlling  the  ICNAI  could  combine  with 
other  objective  functions. 
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Figure  4-6:  One  ICNAl  one  IXUVl  in  an  adaptive  motion  control  simulation.  The  ICNAI 
automatically  adapts  its  position  to  be  in  a  position  during  the  broadcast  which 
minimizes  the  position  uncertainty  of  the  I  All  VI 
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Figure  4-7:  Two  ICNAl  two  IAIJVI  in  an  adaptive  motion  control  simulation.  The 
ICNAk  automatically  adapt  their  position  to  be  in  a  position  during  the  broadcast 
which  minimizes  the  position  uncertainty  of  both  lAUVk 
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Chapter  5 


Experiments 


The  following  chapter  shows  the  results  of  a  series  of  experiments  which  were  part  of 
the  [CN]  research.  As  the  modem’s  capability  to  precisely  measure  the  distance  between 
two  vehicles  outfitted  with  them  is  key  for  all  ICNI  experiments  we  first  carried  out 
an  experiment  to  determine  the  ranging  accuracy  as  well  as  the  maximum  obtainable 
vehicle-vehicle  distance.  In  the  following  experiments  we  collected  the  necessary  data 
to  run  the  localization  algorithms  outlined  in  13.41  in  post  processing  to  asses  their 
performance  and  detect  possible  failure  modes. 

The  first  of  these  experiments  used  surface  crafts  as  ICNAk  such  that  the  algo¬ 
rithms’  results  could  be  compared  against  ground  truth  (IGPSjl.  The  second  and 
third  experiments  involved  two  types  of  underwater  vehicles  using  a  surface  craft  as 
ICNAI  One  was  a  buoyancy  driven  glider,  the  other  a  propelled  IAIJV1  All  three 
vehicles  and  their  capabilities  are  described  in  the  following  sections. 


5.1  Modem  Ranging  Test 

5.1.1  Setup 

All  Cooperative  Navigation  approaches  outlined  in  this  thesis  require  the  participating 
vehicles  to  communicate  their  position  and  obtain  intra- vehicle  range  measurements. 
As  a  result  the  performance  of  all  ICNlalgoritlims  will  be  strongly  dependent  on  the 
performance  of  the  IWHOII  acoustic  modem,  which  will  be  used  for  all  upcoming  ICNI 
experiments.  In  order  to  asses  the  accuracy  of  the  modem’s  ranging  capability  we 
set  up  a  test  in  Lake  Grey,  ME.  with  three  “SCOUT”  lASCk  The  lASCk  are  shown 
in  figure  15-lbl  and  I5~3al  and  described  in  [23] .  They  consist  of  a  commercial  kayak 
hull  outfitted  with  a  thruster,  a  mini- ATX  PC,  [GPS] and  the IWHOT1  acoustic  modem 
which  is  also  used  on  the  I AI J VI;  and  glider.  The  vehicle  dynamics  of  the  IASCI  are 
comparable  to  those  of  a  mid-sized  IAIJVI  By  using  only  the  acoustic  modem  to 
exchange  information  and  estimate  ranges  between  the  two  vehicles,  we  have  applied 
the  same  restrictions  which  are  encountered  in  an  lAUVlonlv  scenario  while  at  the 
same  time  we  are  able  to  compare  the  algorithm’s  navigation  performance  against 
the  “true”  ICPSI  position.  Figure  15-lal  shows  the  modem  transducer  strapped  onto 
the  hull.  This  was  the  configuration  used  during  the  Lake  Grey  test.  Having  the 
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(a)  Modem  transducer  strapped  to  the  hull  (b)  Two  kayaks  moored  on  the  pier  while  the  a 

third  one  moves  and  determines  its  distance  to 
the  moored  ones  using  the  acoustic  modem 


Figure  5-1:  Setup  for  modem  test  on  Lake  Grey,  ME,  October  2004 


transducer  closely  coupled  to  the  vibrating  hull  and  close  to  the  water  surface  limited 
the  maximum  possible  range  to  500  m.  As  a  result,  all  future  experiments  with  the 
lASCk  had  the  transducer  mounted  into  a  towhsh  which  was  hanging  about  2  m  below 
the  keel  (figure  15-81)1) . 

During  the  first  experiment,  two  of  the  lASCk  (“Bobby”  and  “Charlie”)  were 
moored  in  the  open  water  at  a  100  m  distance  while  the  third  kayak  (“Andy”)  first 
moved  away  and  then  toward  the  two  lASCk  in  an  east- west  direction  (top  plot  in 
figure  15421)  .  The  distance  between  “Andy”  and  both  other  lASGli  was  between  100  m 
and  700  m.  During  the  entire  run  “Andy’  alternated  between  sending  query  pings  to 
“Bobby”  and  “Charlie” .  If  a  reply  was  received,  Andy  was  able  to  obtain  the  ITOF1 

5.1.2  Results 

The  moving  IASCI  ( “Andy” )  was  able  to  reliably  obtain  ranging  information  to  both 
vehicles  for  a  distance  of  up  to  400  m.  During  a  short  transition  period  between  400  m 
and  500  m  around  half  of  the  ranging  attempts  were  successful,  but  no  ranging  was 
possible  beyond  500  m  (figure [5721  center  plots).  The  fact  that  the  maximum  obtain¬ 
able  range  was  surprisingly  low  was  mostly  attributed  the  mounting  of  the  transducer 
which,  as  a  result,  was  later  changed  to  the  towfish  configuration  ( figure  l5-3bl) . 

The  two  center  plots  in  figure  15-21  show  the  range  between  “Andy”  and  “Bobby” 
as  determined  by  the  modem  (“o”)  and  by  the  IGPSI  We  first  note  that  the  109 
intra-vehicle  range  measurements  ( “Andy”- “Bobby” :  60;  “Andy” -  “Charlie” :  49)  do 
not  contain  a  single  outlier.  If  the  range  between  the  vehicles  as  derived  by  the  IGPSl 
position  is  taken  as  ground-truth  the  relative  range  error  is  around  1  %. 

We  concluded  that  the  range  accuracy  obtained  in  this  experiment  was  sufficient 
for  Cooperative  Navigation,  but  that  different  transducer  mounting  was  necessary  if 
the  lASCk  were  to  be  used  as  ICNAk 
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Figure  5-2:  Modem  range  test.  One  kayak  (“Andy”)  traveled  on  a  U-shaped  mission 
while  ranging  to  the  two  other  moored  kayaks  (“Bobby”  and  “Charlie”).  The  top 
plot  shows  the  trajectory  of  Andy,  the  bottom  plots  show  the  range  as  determined 
by  the  acoustic  modem  and  the  ground  truth  (ICPSI). 
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(a)  Three  kayaks  navigating  co-  (b)  Towfish  with  modem  transducer 

operatively 


5.2 


CNI  Using  Surface  Crafts  Only 


5.2.1  Setup 

To  obtain  a  data  set  which  also  contained  ground-truth  for  ICNI  in  the  form  of  IGPSI 
measurements,  three  lASGk  as  described  in  the  previous  section  were  set  up  to  run 
in  formation  along  a  trackline  while  broadcasting  their  position  information  over  the 
acoustic  modem.  Each  IASCI  in  the  formation  was  able  to  participate  actively,  by 
sending  information,  and  passively  by  computing  its  position  estimate  based  on  the 
information  obtained  from  the  other  two,  but  the  results  are  only  shown  for  one 
IASCI  of  the  formation.  In  this  case  two  kayaks  act  as  the  ‘IGNAk”  while  the  other 
kayak  acts  as  the  ‘IAUVT .  In  the  setup  shown  in  figure  I5~3al  the  center  kayak  ran  a 
preprogrammed  mission  using  its  IGPSI  for  navigation.  The  other  two  kayaks  followed 
in  a  predetermined  formation  in  order  to  stay  within  range  of  the  acoustic  modems. 
The  position/range-pairs  obtained  from  the  two  1C  NAT  over  the  acoustic  modem  were 
logged  by  the  lAUVlkavak  and  the  algorithm  was  used  to  compute  position  fixes  in 
post-processing.  The  data  set  was  obtained  during  AUVFest  2005  in  Seattle,  WA. 


5.2.2  Results 

Post-processing  the  data  logged  on  the  IASCI  acting  as  a  surrogate  for  an  IAIJVI  we 
computed  the  position  estimate  whenever  a  broadcast  from  any  of  the  two  ICNAk  was 
successfully  received.  Figure  ESI  shows  the  IGPSl  track  of  the  ASC  and  the  computed 
positions  with  their  associated  error  ellipses.  The  tracks  of  the  ICNAk  are  not  shown. 
Figure  EU  shows  the  error  of  the  computed  position,  the  distance  between  the  com¬ 
puted  and  the  IGPSI  position.  When  comparing  the  IGPSI  derived  distance  with  the 
modem  derived  one  we  noticed  larger  errors  for  measurements  obtained  at  position 
#6,  $T5  and  $T6  which  then  led  to  larger  errors  in  the  computed  position. 


Position  Error  [m]  Northings  [in 
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Figure  5-3:  IGPSI  tracks  of  ICNAI  (IASCI)  and  computed  positions 
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Figure  5-4:  Distance  between  IGPSI  derived  position  and  computed  position 
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GPS  fix 
Data  uplink 


\ 

Mission  downlink 


(a)  Seaglider  on  deck  after  recovery  (b)  Seaglider  dive  profile.  Illustration  courtesy 

of  A  PL- University  of  Washington 


Figure  5-5:  University  of  Washington  -  Applied  Physics  Lab’s  Seaglider 


5.3 


ASCk  and  an  underwater  glider 


5.3.1  Setup 

The  second  ICNI  experiment  which  took  place  during  the  MB06  experiment  in  Mon¬ 
terey  Bay,  CA  in  August  2006  involved  two  I  AS  Cl  as  described  in  the  previous  sections 
and  an  underwater  glider  operated  by  the  Applied  Physics  Lab  of  the  University  of 
Washington  (APL-UW)  (see  section. IT.  1 .11  for  a  detailed  description  of  a  glider).  The 
low  power  consumption  (~  1  W)  makes  for  very  long  duration  missions  which  can 
last  up  to  half  a  year.  While  on  the  surface,  the  glider  can  reset  its  navigation  using 
a  IGPS1  but  during  the  dive  the  very  small  power  budget  only  allows  for  very  simple 
navigation  sensors  such  as  a  depth  sensor  and  a  compass.  The  information  from  these 
sensors  together  with  a  vehicle  model  is  used  to  compute  dead-reckoning  navigation 
information.  The  position  estimate  derived  from  these  sensors  can  drift  at  a  rate  of 
up  to  30  %  of  distance  traveled,  especially  when  underwater  currents  are  present.  As 
a  result  the  drift  rate  can  lead  to  a  large  cumulative  navigation  error  during  a  dive 
which  can  typically  last  up  to  several  hours.  This  makes  a  glider  particularly  suited 
for  cooperative  navigation  as  in  a  scenario  with  several  gliders,  a  surfaced  glider  with 
access  to  IGPSI  could  provide  navigation  information  for  every  submerged  glider  within 
communication  range.  While  the  power  consumption  of  an  acoustic  modem  is  very 
high  during  transmission  («  20  W),  only  a  small  number  of  these  transmissions  would 
occur  while  the  glider  is  on  the  surface  which  takes  place  about  every  2  h.  In  receive 
mode  the  power  consumption  drops  to  0.1  W.  As  a  result  an  acoustic  modem  would 
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only  add  about  10-15  %  to  a  glider’s  power  budget.  During  the  MB06  experiment 
a  modem  was  added  to  a  glider  for  the  first  time.  As  the  modem  was  only  capable 
of  logging  information  and  did  not  have  access  to  the  glider’s  main  vehicle  computer 
(which  provides  the  dead- reckoning  information) ,  on-board  processing  was  not  possi¬ 
ble.  The  lASCk  measured  the  range  to  the  glider  and  by  combining  the  logs  from  the 
kayaks,  the  glider’s  Main  Vehicle  Computer  and  the  glider’s  log  of  the  modem  traffic 
it  was  possible  to  compute  post-processed  solutions  of  the  glider’s  positions.  The 
shallow  water  of  Monterey  Bay  prohibited  dives  deeper  than  30  m.  As  the  distance 
traveled  in  horizontal  direction  during  a  single  dive  is  directly  proportional  to  the 
maximum  achievable  depth,  the  depth  limit  only  allowed  for  transects  which  were 
about  100  m  long.  The  main  goal  of  the  experiment  was  to  demonstrate  the  feasibil¬ 
ity  of  glider  communication  for  navigation  purposes.  Future  experiments  will  involve 
longer  and  deeper  dives  leading  to  longer  transsects. 


5.3.2  Results 

As  described  earlier,  the  shallow  depth  of  Monterey  Bay  only  allowed  for  shallow 
diving  depths  and,  as  a  result,  very  short  transects  of  the  glider.  Figure  15-fial  shows 
the  positions  of  the  two  lASCk  (acting  as  ICNAk)  as  well  as  the  dead-reckoned  and 
computed  positions  of  the  glider.  The  inset  shows  a  detailed  view  of  the  glider  track 
(dead-reckoned  and  computed).  The  IGPSl  fixes  mark  the  last  IGPSl  derived  position 
before  the  glider  submerged  as  well  as  the  first  one  after  it  surfaced.  Figures  15-fibl 
through  l5-6dl  show  the  depth,  the  dead-reckoned  position  and  the  computed  position 
of  the  glider  as  well  as  the  associated  uncertainties  for  different  time  instances  during 
the  mission.  Due  to  the  short  transect  the  cumulative  error  of  the  dead-reckoned 
position  is  not  significantly  above  the  uncertainty  of  the  computed  position,  however 
the  computed  position  just  before  surfacing  is  much  closer  to  the  IGPSl  surfacing 
position  than  the  dead-reckoned  one  (figure  l5-6dl) .  Future  experiments  involving 
longer  dives  with  transects  of  several  kilometers  in  length  should  lead  to  significant 
differences  between  the  dead-reckoned  and  the  computed  position. 


5.4  fASCb  and  an  [ATTV 


5.4.1  Setup 

During  a  demonstration  at  the  Naval  Surface  Warfare  Center  ( NSWC )  in  Panama 
City,  FL,  USA  in  December  2006,  two  lASCk  and  a  Bluefin  12”  IAUVI  (figure  ran 
several  missions  in  which  the  lASCk  acted  as  ICNAk  and  followed  the  lAUVl  while  send¬ 
ing  their  IGPSlderived  positions  over  the  acoustic  modem.  The  IAUVI  also  obtained 
distances  to  the  transmitting  IASCI  and  stored  both  information  for  post  processing. 
Ground  truth  was  not  directly  available,  but  by  post-processing  (provided  by  Bluefin) 
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(a)  Glider/ ASC  mission  overview:  The  evolution  of  the  computed  and  dead-reckoned 
position  and  the  associated  uncertainties  are  illustrated  in  the  the  figures  EdThl  through 
lAfidl 
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(b)  Glider/ASC  mission  at  t=123  s.  The  uncertainty  of  the  initial  esti¬ 
mate  of  the  CN  algorithm  is  large  when  compared  to  the  dead-reckoning 
uncertainty. 


data  from  the  sophisticated  and  well  calibrated  sensor  package  and  including  the  po¬ 
sition  obtained  through  the  IGPSl  after  surfacing,  accurate  navigation  information  was 
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(c)  Glider/ASC  mission  at  t=337  s.  The  uncertainty  of  the  computed  posi¬ 
tion  remains  bounded  while  the  uncertainty  of  the  dead-reckoned  estimate 
grows  without  bound. 
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(d)  Glider/ASC  mission  at  t=564  s  (after  surfacing).  The  ground- truth  po¬ 
sition  is  significantly  closer  to  the  computed  position  then  the  dead-reckoned 
position. 


Figure  5-6:  Glider /lASCI  mission. 


available  which  was  used  to  compare  the  results  of  the  ICNI  algorithm. 

A  total  of  16  cooperative  navigation  missions  were  run  during  which  the  I  AT!  VI 
received  the  ICNAls  position  and  measured  the  IGNAIlAUVI  range.  During  these  runs 
the  IAUVI  acted  as  a  master  and  requested  a  new  position  every  30  seconds  switching 
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Figure  5-7:  Two  MIT  ASCs  and  one  Bluefin  12”  IAUVI 


between  the  two  ICNAk.  Of  all  positions  requested  the  IAIJVI  would  receive  about 
60  %.  For  the  remaining  40  %  of  the  queries  the  ICNAI  did  either  not  receive  the 
request  or  the  lAUVi  did  not  receive  the  ICNAI s  answer.  Sometimes  the  lAUVI  would 
also  suspend  requesting  positions  because  it  needed  to  transfer  other  mission  specific 
information  over  the  acoustic  modem.  As  a  result  the  update  rate  for  position/range 
pairs  was  about  one  per  minute. 

After  requesting  a  position/range  from  both  ICNAk.  the  IAIJVI  would  send  its  own 
position  estimate  over  the  acoustic  modem.  Furthermore,  the  ICNAk  would  contin¬ 
uously  broadcast  their  IGPSIderived  position  over  the  radio  such  that  both  ICNAk 
were  aware  of  where  the  other  one  is.  Knowing  where  the  IAIJVI  and  the  other  ICNAI 
is,  enabled  the  ICNAk  to  follow  the  IAIJVI  in  a  way  that  was  optimal  for  cooperative 
navigation: 

•  In  order  to  maintain  optimal  acoustic  communication,  the  IAIJVI  would  try  to 
stay  150  m  behind  the  IAIJVI 

•  To  minimize  the  covariance  of  the  computed  solution  the  ICNAk  would  try  to 
form  a  right-angled  triangle  with  the  IAIJVI  in  the  corner  with  the  right  angle 
and  the  ICNAk  in  the  other  two. 

As  the  I  AT  J  Vt  s  position  updates  were  received  at  a  rate  of  only  0(l/min),  it  was 
very  difficult  for  the  ICNAk  to  maintain  the  triangular  formation  when  the  straight 
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Figure  5-8:  I  AT  I  VllASCl  mission  1:  Dead-reckoned  track  and  computed  positions  of 
I  All  VI IHFS1  positions  of  ICNAk  Inset:  Detailed  view  of  position  #8. 


transects  were  short  (figure  15-81) .  During  the  second  mission  (figure  15-91)  ICNAIl  was 
able  to  maintain  an  aft-starboard  position  with  respect  to  the  I  All  VI  while,  ICNAk 
maintained  an  aft-port  position.  Even  when  the  formation  was  not  maintained  the 
lAUVfs  broadcast  enabled  the  ICNAk  to  stay  close  enough  to  maintain  the  acoustic 
communication  channel.  The  navigation  error  was  modeled  using  sensor  noise  as 
provided  in  EH.  While  the  results  for  only  two  runs  are  shown  in  figure  15-81  and 
figure  15-91  the  quality  of  the  results  computed  by  the  algorithm  was  the  same  for  all 
16  runs. 


5.4.2  Results 

Figure  15-81  and  figure  15-91  show  two  of  the  missions  carried  out.  The  first  mission 
consisted  of  a  U-shaped  trackline  of  about  1  km  length.  After  initializing  its  position 
with  IGPSI  the  IAUVI  submerged  to  a  depth  of  about  12  m  and  ran  the  mission  at 
a  constant  speed  of  1.5  m/s.  The  detail  in  figure  f5dsl  shows  the  computed  position 
#8  and  its  covariance  ellipse.  Also  shown  is  the  “ground-truth”  track  as  well  as 
the  “ground-truth”  position  estimate  at  the  time  of  the  computed  solution.  As  the 
“ground-truth”  position  is  based  on  post-processed  dead-reckoning  data  the  distance 
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Figure  5-9:  I  At  I  VllASCl  mission  2:  Dead-reckoned  track  and  computed  positions  of 
IAUV1  [HPSI  positions  of  ICNAk  Inset  (top):  Detailed  view  of  mission  end  (positions 
#31,  #32  and  #33)  ;  Inset  (bottom):  Detailed  view  of  mission  start  (positions  #9 
through  #13). 


between  it  and  the  computed  position  can  only  provide  a  qualitative  assessment  of 
the  algorithm’s  performance.  As  a  result  we  did  not  compute  the  Euclidean  distance 
between  the  two  positions.  Also,  the  post-processed  track  is  the  result  of  a  non-linear 
optimization  so  no  covariance  estimate  can  be  provided. 

The  second  mission  consisted  of  a  4  km  east-to-west  trackline.  During  this  mission 
the  kayaks  were  able  to  maintain  the  triangular  formation  for  most  of  the  time.  On 
five  occasions  during  this  mission  the  IAIJVI  would  spend  four  minutes  transmitting 
mission  specific  data.  During  this  time  no  positions  were  queried  from  the ICNAl which 
lead  to  the  wide  gaps  between  the  computed  solutions  (e.g.  between  #19  and  #20 
as  well  as  #27  and  #28).  The  two  insets  in  figure  E31  show  two  magnified  views  of 
the  track  at  the  same  scale.  The  bottom  one  near  the  beginning  (eastern  end)  of 
the  mission  and  the  top  one  of  the  end  (west).  These  illustrate  how  beneficial  the 
information  from  the  ICNAk  is  for  navigation  accuracy.  In  the  beginning  the  dead- 
reckoned  position  is  very  close  to  the  “ground-truth”  and  the  computed  solution 
while  at  the  end  of  trackline  the  “ground-truth”  as  well  as  the  computed  position 
have  consistently  moved  away  from  the  dead-reckoned  position.  The  dead-reckoning 
error,  represented  by  the  growing  error  ellipse,  depends  on  the  distance  traveled  and 
will  grow  without  bound  if  the  IAIJVI  is  submerged,  while  the  error  of  the  computed 
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Figure  5-10:  IAIJV|/fASCI  mission  2  with  falsified  range  measurement  at  k  =  5:  The 
erroneous  range  measurement  causes  a  localization  error  for  all  3  algorithms  at  k  =  5, 
but  while  the  CN  algorithm  has  fully  recovered  at  the  next  step,  IEKFI  and  particle 
filter  only  converge  slowly  towards  the  correct  solution. 


solution  only  depends  on  the  position  error  of  the  ICNAk  and  the  geometry.  It  is 
bounded  if  the  position  error  of  the  ICNAk  is  bounded  and  if  positions  which  were 
computed  from  collinear  or  near  collinear  geometries  are  filtered  out.  Toward  the  end 
of  the  second  mission,  the  ICNAk  were  not  able  to  keep  up  with  the  I AU Vl  which  led  to 
less  favorable  geometries  resulting  in  slightly  larger  error  covariances  of  the  computed 
solution  than  in  the  beginning.  As  in  the  first  mission,  the  algorithm’s  performance 
is  hard  to  quantify.  Qualitatively,  the  computed  solutions  are  consistently  very  close 
to  the  “ground-truth”  throughout  the  entire  track  while  the  dead-reckoned  position 
drifts  over  time. 


5.5  Comparison  with  Bayesian  Estimators 

In  order  to  compare  the  performance  of  our  CN  algorithm  with  common  classical 
approaches,  an  IEKFI  and  alPFIwith  300  particles,  we  computed  the  position  using  all 
three  methods  at  each  time  instant  k  when  a  new  range/position  pair  was  available. 
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Figure  5-11:  Position  error  for  ICNl  lEKFI  and  IPFI  for  the  tracks  shown  in  figure  15-101 
Top:  position  error  for  CN,  lEKFI  and  Particle  filter.  Bottom:  cost  function  for 
CN  showing  a  very  large  peak  at  k  =  5  which  would  enable  one  to  filter  out  this 
inconsistent  range  measurement. 


Because  of  the  high  quality  dead-reckoning  measurements  and  absence  of  range  mea¬ 
surements  outliers  in  the  available  kavak/fXUVI  data  sets,  each  of  the  three  methods 
performed  similarly  and  the  results  were  within  the  accuracy  of  the  ground  truth. 

Large  underwater  range  measurement  outliers  can  occur  in  more  challenging  ex¬ 
perimental  scenarios.  In  such  a  scenario  the  Gaussian  noise  assumption  does  not 
hold  [66].  For  this  reason  we  simulated  a  typical  outlier  measurement  by  using  the 
mission  shown  in  figure  lA8l  and  setting  the  range  measurement  obtained  by  the  IAUVI 
at  k  =  5  from  r( 5)  =116.86  m  to  r(5)  =60  m.  All  subsequent  range  measurements 
were  unchanged.  The  computed  tracks  are  shown  in  figure  15-101  Upon  receipt  of  the 
fifth  measurement  the  error  of  the  position  estimate  “jumps”  for  all  three  methods, 
most  significantly  for  the  ICNl  algorithm.  However  at  k  =  6  the  ICNl  algorithm  in¬ 
stantly  recovers  to  the  correct  position,  while  the  lEKFI  and  particle  filter  only  slowly 
converge  towards  the  correct  path.  This  is  due  to  the  very  low  measurement  update 
frequency.  The  erroneous  position  produced  by  our  ICNl  algorithm  at  k  =  5  is  par¬ 
ticularly  large  because  our  approach  may  only  select  from  the  solution  set  provided 
in  S(5)  (see  appendix  13 .5 .31).  This  range  measurement  is  however  inconsistent  with 
the  previous  range  measurements  and  the  dead-reckoned  track  and  as  a  result  has  a 
much  higher  accumulated  cost  C(5),  shown  as  a  single  peak  in  figure  EH]  Therefore 
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it  would  be  possible  to  use  C  to  detect  and  filter  out  false  range  measurements. 

In  summary  an  lEKFlis  unsuitable  for  this  application.  However  a  more  advanced 
particle  filter  with  a  sufficiently  large  number  of  particles  could  possibly  provide 
similar  performance  to  our  proposed  algorithm. 
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Chapter  6 
Conclusion 


In  order  to  maintain  a  bounded  position  error,  an  individual  underwater  vehicle  has 
to  occasionally  surface  for  a  IGPSlfix  or  operate  within  a  small  area  surrounded  by 
pre-deployed  localization  equipment.  This  is  independent  of  the  vehicle’s  size  and  the 
sophistication  of  its  navigation  sensors.  Cooperative  Navigation  provides  a  framework 
for  future  deployments  which  cannot  rely  on  external  infrastructure  and  it  minimizes 
the  number  of  surfacings  as  long  as  two  or  more  vehicles  operate  sufficiently  close  to 
each  other. 


6.1  Contributions 

This  thesis  starts  by  examining  the  constraints  which  the  underwater  domain  im¬ 
poses  on  Cooperative  Navigation.  Its  first  major  contribution  is  an  algorithm  which 
is  designed  to  take  these  constraints  into  account.  The  algorithm  provides  a  robust 
estimate  of  the  vehicle’s  location  when  supplied  with  navigation  information  from 
cooperating  platforms.  It  was  extensively  tested  in  a  series  of  experiments  using  Au¬ 
tonomous  Surface  Crafts,  propelled  Autonomous  Underwater  Vehicles  and  a  buoyancy 
driven  glider.  The  results  from  these  experiments  show  that  it  compares  favorably 
against  the  two  classical  methods  which  are  also  presented  in  this  thesis. 

For  a  group  of  vehicles  sharing  information,  two  cooperation  strategies  can  be 
devised.  The  first  one  does  not  assume  any  structure  in  the  flow  of  navigation  infor¬ 
mation.  All  vehicles  occasionally  broadcast  their  position  estimate  and  incorporate 
messages  which  they  receive  from  others.  The  second  one  assumes  a  hierarchy  in 
which  only  a  special  subset  of  vehicles  broadcasts  information  which  all  others  use, 
thereby  enforcing  a  uni-directional  flow  of  information.  Our  algorithm,  as  well  as 
the  two  classical  approaches  we  present,  can  work  in  both  scenarios  without  modi¬ 
fication.  If  a  vehicle  however  uses  information  from  another  one  to  update  its  own 
position,  the  estimates  between  the  two  vehicles  become  cross-correlated.  This  is  not 
a  problem  as  long  as  it  remains  a  uni-directional  cross-correlation.  However  in  the 
case  of  the  first  scenario,  in  which  the  flow  is  omni-directional,  information  shared  by 
a  vehicle  may  at  a  later  time  be  presented  to  that  vehicle  again  as  the  information 
has  been  incorporated  by  others  which  now  broadcast  their  position  estimate.  For  all 
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navigation  algorithms  presented,  this  can  lead  to  an  over-confidence  in  the  position 
estimate  which  can  cause  all  of  them  to  diverge. 

An  algorithm  that  ensures  that  the  cross-correlations  remain  uni- directional,  re¬ 
gardless  of  the  direction  of  the  information  flow,  and  thereby  prevents  over-confidence, 
is  the  second  major  contribution  of  this  thesis.  It  can  be  combined  with  any  localiza¬ 
tion  algorithm  we  present.  While  the  algorithm  requires  additional  information  to  be 
transmitted  as  well  as  additional  computation  we  show  how  omitting  this  step  can 
cause  the  navigation  filters  of  all  participating  vehicles  to  diverge  even  in  a  very  simple 
simulated  scenario.  We  also  propose  an  extension  of  the  algorithm  which  trades  off 
computation  and  bandwidth  requirements  against  the  improvement  in  the  position 
estimate  obtained  through  cooperation. 

The  quality  of  the  position  estimate  obtained  through  measurements  to  beacons 
at  known  locations,  in  our  case  broadcasting  vehicles,  depends  on  the  relative  po¬ 
sition  between  the  beacons  and  the  estimating  vehicle.  This  is  independent  of  the 
algorithm.  The  hierarchical  scenario,  in  which  the  objective  of  the  beacon  vehicles 
is  to  minimize  the  position  uncertainty  of  all  others,  we  can  control  the  geometry 
by  actively  positioning  the  beacon  vehicles.  Our  third  contribution  is  a  distributed 
algorithm  which  runs  on  all  beacon  vehicles  and  positions  them  such  that  they  jointly 
minimize  the  the  uncertainty  of  all  receiving  vehicles.  The  algorithm  does  not  assume 
a  central  controller  and  instead  only  relies  on  information  which  is  locally  available. 


6.2  Future  Work 

Autonomous  Underwater  Vehicles  have  been  successfully  deployed  for  over  a  decade, 
but  only  the  last  few  years  have  seen  experiments  involving  several  at  a  time.  Even  in 
the  few  experiments  which  involved  multi- vehicle  deployments,  the  individual  mem¬ 
bers  of  the  group  were  often  not  aware  of  each  other  and  only  communicated  to  a 
central  control  station  or  lacked  underwater  communication  equipment  altogether. 
As  a  result,  only  a  very  limited  amount  of  data  sets  are  available  which  can  be  used 
to  test  algorithms  by  post-processing  data.  Even  fewer  experiments  have  been  carried 
out  specifically  with  cooperation  in  mind,  be  it  for  navigation  or  any  other  purposes. 
More  recently  however  an  acoustic  modem  with  access  to  a  globally  synchronized 
time  signal,  the  key  piece  of  equipment  for  the  successful  implementation  of  any  ICNI 
algorithm,  has  become  a  standard  feature  on  all  new  marine  platforms,  and  many 
older  ones  have  been  retrofitted  with  this  equipment.  In  addition,  great  progress  was 
made  in  standardizing  the  communications  protocol  to  ensure  that  a  heterogeneous 
group  of  vehicles  can  successfully  share  information  [82] .  Experiments  planned  for 
the  near  future  will  offer  many  opportunities  to  verify  and  test  the  proposed  algo¬ 
rithms  in  real-time.  Results  from  these  experiments  will  drive  future  research.  As  a 
navigation  error  can  lead  to  the  loss  of  a  multi- million  dollar  vehicle,  most  research 
for  cooperative  navigation  algorithms  will  focus  on  robustness.  The  emergence  of 
new  platforms  and  sensors  not  considered  in  this  thesis  will  lead  to  new  possibilities 
as  well  as  constraints  to  which  the  navigation  algorithms  have  to  adapt.  For  the 
hierarchical  approach  which  has  a  set  of  dedicated  beacon  vehicles  there  is  a  lot  of 
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room  for  future  research  to  optimize  the  path  of  these  vehicles.  Due  to  the  strong 
variability  of  the  acoustic  communication  channel,  maximizing  the  likelihood  that  a 
transmission  is  successful,  is  a  necessary  additional  objective.  Recent  advances  in 
predicting  the  modem  performance  in  real-time  from  in  situ  measurements  m  can 
provide  additional  information  which  is  used  in  the  path  planning  process. 

With  the  necessary  hardware  infrastructure  being  available  on  all  underwater 
platforms  soon,  a  mature  set  of  cooperative  navigation  algorithms  will  become  a 
commodity  in  underwater  navigation  ensuring  that  every  vehicle  uses  all  available 
information  to  localize  itself. 
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Appendix  A 

Coordinate  Systems 

A.l  Reference  Frames 


(a)  World  Coordinate  System  (b)  Vehicle  Coordinate  System 
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A. 2  Vehicle  Body  Pose  and  Velocities 


Table  A.l:  Vehicle  Body  Pose 


Linear 

Angular 

X  = 

1  1 

(j)  (roll) 

ip  (pitch) 

9  (yaw) 

9  (heading)  ;  9  =  f  —  9 

Table  A. 2:  Vehicle  Body  Velocities 


Linear 

Angular 

u 

u=xv =^f-  (forward  velocity) 

0=^  (roll  rate) 

u  — 

V 

v=yv =^%-  (starboard  velocity) 

(pitch  rate) 

w 

w=zv =%-  (vertical  velocity) 

9—j ^  (yaw  rate) 
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